From 12bff9187e9043cfb2fae62f49fe93fef880f1ce Mon Sep 17 00:00:00 2001 From: Dan Gottlieb Date: Wed, 29 Apr 2026 13:08:36 -0400 Subject: [PATCH] RSDK-13851: Apply PoseCloud to all intermediate linear constraint waypoints. --- motionplan/armplanning/plan_manager.go | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/motionplan/armplanning/plan_manager.go b/motionplan/armplanning/plan_manager.go index f2ec367b625..e9f0827d74a 100644 --- a/motionplan/armplanning/plan_manager.go +++ b/motionplan/armplanning/plan_manager.go @@ -276,12 +276,11 @@ func (pm *planManager) generateWaypoints(ctx context.Context, start, goal refere } toPose := spatialmath.Interpolate(start[frameName].Pose(), pif.Pose(), by) - if i < numSteps { - to[frameName] = referenceframe.NewPoseInFrame(pif.Parent(), toPose) - } else { - // If this is the last step, copy over any goal cloud the plan request declared. - to[frameName] = referenceframe.NewPoseInFrameWithGoalCloud(pif.Parent(), toPose, pif.GoalCloud) - } + // Dan: We copy the PoseCloud over to each intermediate (and final goal) waypoint. We do + // this thinking that if the PoseCloud describes a set of desirable orientations to make + // IK easier, we would like all of the intermediate waypoints to also be more easily + // solved for. + to[frameName] = referenceframe.NewPoseInFrameWithGoalCloud(pif.Parent(), toPose, pif.GoalCloud) } waypoints = append(waypoints, to)