interpolateVel: velocity smoother does not appear to work correctly

[gavanderhoorn]

@gavanderhoorn wrote in #16 (comment):

I'm also slightly confused how that conditional is supposed to work, as mDesc.vel is updated here:

staubli_val3_driver/staubli_val3_driver/val3/ros_server/pushMotion.pgx

Line 42 in 17fd14a

l_mTrajPt.mDesc.vel= limit((rosTrajPtMsg.jointTrajPt.nVelocity * 100), 0, 100)

to contain a percentage (ie: int) and no longer the float we transmit in the simple message msg. The check in the motionControl task (which calls interpolateVel(..)) checks for mDesc.vel == 0.1, which can never be true if I understand things correctly.

The conditional is here:

staubli_val3_driver/staubli_val3_driver/val3/ros_server/interpolateVel.pgx

Lines 14 to 17 in 17fd14a

	// Smooth last point in traj
	if((x_mMotion.mDesc.vel == 0.1) and (x_mMotion.nSequence != 0))
	x_mMotion.mDesc.vel = (nVel[0] - (nVel[1] - nVel[0]))
	endIf

As noted in the quote above: mDesc.vel isn't actually between (0, 1] here any more, but is converted to a percentage (ie: (0, 100]). As such, the condition will never be true, causing the quoted code snippet to never actually be executed.

[gavanderhoorn]

@marshallpowell97: could you take a look?

Perhaps the approach taken in interpolateVel could be changed?