@@ -394,47 +394,6 @@ func NewSlerpOrientationConstraint(start, goal spatial.Pose, tolerance float64)
394394 return validFunc , gradFunc
395395}
396396
397- // NewPlaneConstraint is used to define a constraint space for a plane, and will return 1) a constraint
398- // function which will determine whether a point is on the plane and in a valid orientation, and 2) a distance function
399- // which will bring a pose into the valid constraint space. The plane normal is assumed to point towards the valid area.
400- // angle refers to the maximum unit sphere segment length deviation from the ov
401- // epsilon refers to the closeness to the plane necessary to be a valid pose.
402- func NewPlaneConstraint (pNorm , pt r3.Vector , writingAngle , epsilon float64 ) (StateConstraint , motionplan.StateMetric ) {
403- // get the constant value for the plane
404- pConst := - pt .Dot (pNorm )
405-
406- // invert the normal to get the valid AOA OV
407- ov := & spatial.OrientationVector {OX : - pNorm .X , OY : - pNorm .Y , OZ : - pNorm .Z }
408- ov .Normalize ()
409-
410- dFunc := motionplan .OrientDistToRegion (ov , writingAngle )
411-
412- // distance from plane to point
413- planeDist := func (pt r3.Vector ) float64 {
414- return math .Abs (pNorm .Dot (pt ) + pConst )
415- }
416-
417- // TODO: do we need to care about trajectory here? Probably, but not yet implemented
418- gradFunc := func (state * motionplan.State ) float64 {
419- pDist := planeDist (state .Position .Point ())
420- oDist := dFunc (state .Position .Orientation ())
421- return pDist * pDist + oDist * oDist
422- }
423-
424- validFunc := func (state * motionplan.State ) error {
425- err := resolveStatesToPositions (state )
426- if err != nil {
427- return err
428- }
429- if gradFunc (state ) < epsilon * epsilon {
430- return nil
431- }
432- return errors .New (planarConstraintDescription + " violated" )
433- }
434-
435- return validFunc , gradFunc
436- }
437-
438397// NewLineConstraint is used to define a constraint space for a line, and will return 1) a constraint
439398// function which will determine whether a point is on the line, and 2) a distance function
440399// which will bring a pose into the valid constraint space.
@@ -621,26 +580,6 @@ func CreateSlerpOrientationConstraintFS(
621580 return constraintInternal .constraint , constraintInternal .metric , nil
622581}
623582
624- // CreateLineConstraintFS will measure the linear distance between the positions of two poses across a frame system,
625- // and return a constraint that checks whether given positions are within a specified tolerance distance of the shortest
626- // line segment between their respective positions, as well as a metric which returns the distance to that valid region.
627- func CreateLineConstraintFS (
628- fs * referenceframe.FrameSystem ,
629- startCfg referenceframe.FrameSystemInputs ,
630- from , to referenceframe.FrameSystemPoses ,
631- tolerance float64 ,
632- ) (StateFSConstraint , motionplan.StateFSMetric , error ) {
633- // Need to define a constructor here since NewLineConstraint takes r3.Vectors, not poses
634- constructor := func (fromPose , toPose spatial.Pose , tolerance float64 ) (StateConstraint , motionplan.StateMetric ) {
635- return NewLineConstraint (fromPose .Point (), toPose .Point (), tolerance )
636- }
637- constraintInternal , err := newFsPathConstraintTol (fs , startCfg , from , to , constructor , tolerance )
638- if err != nil {
639- return nil , nil , err
640- }
641- return constraintInternal .constraint , constraintInternal .metric , nil
642- }
643-
644583// CreateAbsoluteLinearInterpolatingConstraintFS provides a Constraint whose valid manifold allows a specified amount of deviation from the
645584// shortest straight-line path between the start and the goal. linTol is the allowed linear deviation in mm, orientTol is the allowed
646585// orientation deviation measured by norm of the R3AA orientation difference to the slerp path between start/goal orientations.
0 commit comments