(Joint) trajectory following capability
Issue #12
new
Various planners generate joint trajectories including position, velocity, acceleration (in some cases even effort?) per knot point. While it is possible to send such trajectories over the current API by repeatedly sending new set points, this approach throws away a lot of available information about the trajectory and thus provides only limited tracking capability.
I assume that there already is internal discussion about this at IHMC, but wanted to have this also tracked here.
Comments (18)
-
-
reporter
To provide some more details about how things are frequently done in the ROS world:
The standard (or at least most frequently used) trajectory representation is the trajectory_msgs/JointTrajectory message. It contains a vector of trajectory_msgs/JointTrajectoryPoint messages that provide position and optionally velocity and accelerations for timestamped waypoints.
The JointTrajectoryController is a controller that can be used to execute such trajectories for groups of joints. It internally computes quintic splines between waypoints (if position, vel, acc are set) and samples these splines at control rate to generate actuator desired values. As described in Available_interfaces, it provides a topic interface (which just executes incoming trajectory messages) and a action interface (which executes incoming trajectory action and provides feedback about how things are going). A useful feature is the trajectory replacement capability which allows specifying a trajectory to start at some point in the future based on the message's timestamp. Using this feature, multiple trajectories can be sent to different controllers (for example l_arm_controller, torso_controller and r_arm_controller) and execute synchronously.
I'm not saying all of the above needs to be (re-)implemented, but wouldn't complain about it either :D
Of course, there are many scenarios (reaching a desired endeffector pose in world frame) where joint space trajectories for single appendage chains are of limited utility on a biped system due to the floating base and the need to obey balance constraints.
-
reporter
@georgwi @dljsjr I saw recent additions towards joint trajectories (commit). As mentioned previously, there already exists a standard trajectory_msgs/JointTrajectory message that is very similar to the ihmc_msgs/ArmJointTrajectoryPacketMessage.msg you created. It is pretty much trivial to convert between the two, but if you provide the standard ROS trajectory interface directly, it would likely make things more convenient for users.
-
@Stefan_Kohlbrecher We will probably try to do this. The use of the custom message is because right now all of our ROS bindings map one-to-one to our Java IPC data structures with a very simple translator living in between the ROS API and the rest of out code. So this message needs to exist regardless.
-
Just to jump in here months after the fact, but I've been trying to bridge our front end tools with the ihmc_ros API (and sim). I noticed there are two likely control topics for this in the current version: arm_joint_trajectory and arm_joint_trajectory2. The former is the custom message type described above, the latter is a standard JointTrajectory message.
I did the obvious thing and tried to send something to arm_joint_trajectory2, but the robot didn't respond. Before I dove into this too much, i wanted to make sure that the behavior i'm expecting (and the way to achieve it) is reasonable, or if there is something i am doing wrong.
-
as a quick follow up, I'm using MoveIt! to generate trajectories, and it doesnt look like they have any velocity component (i.e., vels are all 0). maybe this is the issue. seems strange, but i guess i can do the interpolation myself.
-
@swhart115 This is mostly the domain of @georgwi but from my understanding and a cursory glance at the underlying Java code it seems that if you send a trajectory with a velocity of 0.0 then no motion would be the expected behavior.
-
@dljsjr Yeah, that's what i suspected once i realized this. Thanks, I can fix this on my side.
I'm assuming that if i do this right, either of the two arm_joint_trajectory topics will suffice, correct? There's no other mode or state changing i have to do beyond the default (which i believe is "walking" mode)?
-
@swhart115 This is correct; either topic should work and you only need to switch "modes" if you want to do whole-body position control (e.g. car egress type stuff).
-
Great, thanks. (and i hope never to have to do "car egress type stuff again." at least with atlas :) )
-
This has been a while and I do not have the code here right now. However, if I remember correctly there should be a movement even when the desired velocity is zero. The velocity provided is just the desired velocity at the waypoint time so the movement should stop briefly at every waypoint but the robot should do something regardless. (The Acceleration and Effort fields have no effect)
Are you checking the ihmc error topic? If the controller rejects the trajectory you should see something there.
-
@georgwi Okay, it looks like i am getting some errors:
header: seq: 0 stamp: secs: 255 nsecs: 507000007 frame_id: '' level: 8 name: '' msg: MultipleWaypointsOneDoFJointTrajectoryGenerator can not append waypoint: time for consecutive waypoints must be increasing. MultipleWaypointsOneDoFJointTrajectoryGenerator can not append waypoint: time for consecutive waypoints must be increasing. MultipleWaypointsOneDoFJointTrajectoryGenerator can not append waypoint: time for consecutive waypoints must be increasing. MultipleWaypointsOneDoFJointTrajectoryGenerator can not append waypoint: time for consecutive waypoints must be increasing. MultipleWaypointsOneDoFJointTrajectoryGenerator can not append waypoint: time for consecutive waypoints must be increasing. MultipleWaypointsOneDoFJointTrajectoryGenerator can not append waypoint: time for consecutive waypoints must be increasing. MultipleWaypointsOneDoFJointTrajectoryGenerator can not append waypoint: time for consecutive waypoints must be increasing. file: '' function: '' line: 0 topics: [] ---Time seems to be increasing in a reasonable way from my input trajectory. I converted the ros::Duration() that was in the "time_from_start" field of the original JointTrajectory() message to seconds. Here it is from an command line echo (with some trimming):
robot_side: 0 trajectory_points: - positions: [0.0332243196290101, 0.19105733316963192, 2.9099931280306293, 0.4183768085114311, 0.064777433668767, -0.21678491752647813, -2.9671] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] time: 0.2 - positions: [0.0332243196290101, 0.19105733316963192, 2.9099931280306293, 0.4183768085114311, 0.064777433668767, -0.21678491752647813, -2.9671] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] time: 0.4 - positions: [0.04542815475533276, 0.23131113798258424, 2.8690082325558666, 0.5288197130640705, 0.11604243831057118, -0.27652451230426606, -2.9671] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] time: 0.6 - positions: [0.054856628376441265, 0.25914175326050204, 2.839688958789066, 0.61429564841955, 0.15678548121316468, -0.32331578997581095, -2.9671] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] time: 0.8 - positions: [0.06233335953080165, 0.28018818310916827, 2.817068414060477, 0.6861354654520642, 0.19186282771899782, -0.36305549347131905, -2.9671] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] time: 1.0 - positions: [0.06820979300605133, 0.29679317529704946, 2.799111807369814, 0.7490034598676338, 0.2232513685568974, -0.39820262643266735, -2.9671] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] time: 1.199999999 - positions: [0.07268069344862213, 0.31020179895363315, 2.784732922142824, 0.8053605682824809, 0.2519809731096324, -0.43007222461191186, -2.9671] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] time: 1.399999999 ... -
The time seems to be increasing to me! We might have introduced a bug there. I will look into this - thanks for the feedback!
-
@georgwi Sure, no problem. Also, for completeness in this discussion, i just tired adding velocity terms to the trajectory, but there was no discernible difference (i.e., no movement, and similar error messages).
-
So I tried publishing something similar to your trajectory. There is a safety feature that wont allow joint speeds of over 1rad/s (this is unfortunately not published in the error topic but in the regular console output of the controller). If you start the robot far of your first waypoint then the speed when going there in 0.2s will be very high. Now the controller will increase that time until the joint speed does not exceed 1rad/s anymore. If that takes 2.0s then your next waypoint at 0.4s will be in the past and you will see the error you got.
Could that be what happened to you? Could you try publishing a really slow trajectory? These safety features are not very smart - we just put them in to not constantly break our new forearms when we got them. If that indeed caused your problem and you want that safety to behave differently I am sure we can change that for you.
-
@georgwi yeah, that was it. I slowed down the time, and the arm moved as it now had enough time to reach the goals. However, this test made apparent that, for some reason, my MoveIt! trajectories are always trying to start at the 0 position, not the current robot position, which is why the first few waypoints required such a high velocity to reach (they were far away). I will track down why this is, but it's likely a problem on my (or moveit's) end from something getting initialized weird.
Thanks for the help!
-
yes, MoveIt! was expecting /joint_states, not /ihmc_ros/atlas/output/joint_states. relaying makes it work. Looks like i'm good to go!
-
reporter
For anyone interested, I implemented a small converter node that maps the two standard interfaces as used in the JointTrajectoryController (namely the command topic and the FollowJointTrajectoryAction action interface to the
/ihmc_ros/atlas/control/arm_joint_trajectory2topic. It's bit rough so far (for instance doesn't check for trajectory execution success correctly yet ;) ), but sending trajectories via standard Actions works fine already. You can find it here: https://github.com/team-vigir/vigir_ihmc_integration/tree/master/vigir_ihmc_control_integration - Log in to comment
Thanks @Stefan_Kohlbrecher. We are in fact making some progress in this area, and we will keep BitBucket updated.