Velocity and acceleration of swing foot are represented in incorrect frame

It seems that target velocity and acceleration of SurfaceTransformTask for swing foot are passed with represented in incorrect frame in the following:

lipm_walking_controller/src/states/SingleSupport.cpp

Lines 151 to 152 in b564d65

    
           swingFootTask->refVelB(swingFoot_.vel()); 
        
           swingFootTask->refAccel(swingFoot_.accel());

SurfaceTransformTask requires velocity and acceleration represented in the surface frame.

But, it seems these values are represented in world frame.
For example, looking at xyTakeoffChunk_ of SwingFoot, first the start and end positions in the world frame are passed to RetimedPolynomial<QuinticHermitePolynomial, Eigen::Vector2d>:

lipm_walking_controller/src/SwingFoot.cpp

Line 61 in b564d65

    
           xyTakeoffChunk_.reset(initPos.head<2>(), aerialStartPos.head<2>(), takeoffDuration_);

and then, the interpolated velocity and acceleration are set:

lipm_walking_controller/src/SwingFoot.cpp

Lines 133 to 134 in b564d65

    
           vel_.head<2>() = xyTakeoffChunk_.vel(t1); 
        
           accel_.head<2>() = xyTakeoffChunk_.accel(t1);

so they should be represented in world frame.

So far, I haven't encountered any actual problematic behavior related to this issue.

Nice catch! 🎣 You're absolutely right, the world-to-surface transform is missing 🙈 The linear velocity and acceleration vectors will point in the wrong directions unless the surface frame is aligned with the world frame.

Thank you for your reply. I made PR: arntanguy#2. Since I'm currently using the branch of #23, I posted the PR to that branch

Close this because arntanguy#2 is merged.

	swingFootTask->refVelB(swingFoot_.vel());
	swingFootTask->refAccel(swingFoot_.accel());

	vel_.head<2>() = xyTakeoffChunk_.vel(t1);
	accel_.head<2>() = xyTakeoffChunk_.accel(t1);