Stable Joint Motion - #3
Conversation
…ulation into alexnavtt/stable_joint_motion
…e positives with body "collisions"
…e motions in sparse trajectories
… when used with joint waypoints
…cartesian motion with joint position action server terminates
…er terminal states
…ulation into alexnavtt/stable_joint_motion
…tdown while arm is deployed
FabianEP11
left a comment
FabianEP11
left a comment
There was a problem hiding this comment.
Maybe I didn't track this down completely, but where is the feedback propagation happening?
I saw the publish_feedback for ArmCartesianCommand, but couldn't find where it gets forwarded to StableArmCommand.
|
I'll admit that after trying for a little while, it just seemed like the feedback wouldn't be meaningful since it's actually a sequence of commands being issued under the hood, and not just one command, so updates on distance to goal and tracking error wouldn't mean much. So in short, there is no feedback in that case |
|
Alright, I removed the odom frame requirement and tested it, all is well. I assume that when I made that requirement I was rushing to get something working, and I just put that in there as a guardrail against my own mistakes. Also fixed the other issues |
4 participants
The PR adds the ability for the robot to perform "Stable" arm motions. By this I mean that the end-effector and intermediate states are identical between the planned and executed trajectories.
The issue previously is that MoveIt outputs joint trajectories which assume a static base, but the robot leans its body during execution to maintain balance. The result is that executed trajectory and the final end-effector position are slightly off by however much the robot leaned, so poses far away from the robot body would be less accurate that those close to it.
The solution here is that when MoveIt outputs a joint trajectory, instead of executing it we calculate the end-effector position at each waypoint and create a new trajectory that contains both Cartesian and joint waypoints. This allows us to invoke the Spot API CartesianCommand, which accepts a final joint configuration argument. To maintain accuracy during the move, we command the robot to move to each waypoint in the trajectory in turn with its unique joint configuration instead of providing the entire trajectory in a single monolithic command.
Note that since the leaning still occurs, the final joint configuration is slightly different from that commanded by MoveIt