Cartesian teleoperation in MoveIt

[Flavian Hautbois]

Hello everyone,

I have been using MoveIt with the Motoman SIA 10D for a few days, it works great! Congratulation to the people from I-ROS and MoveIt.
However my main application was not doing path planning, but "real-time" teleoperation (best effort actually, I'd like to achieve ~50Hz).
This is my setup: I launch the MoveIt application with the controller for my robot (launch file enclosed) and I have a node that publishes a cartesian goal (position + speed).

I have several questions:

1- what message type should I use for the cartesian goal, and were should I publish it? (I was thinking of publishing it as a child to base_link in tf)

2- I am using the constraint aware kinematics solver to compute joint trajectories ; I want to compute IK position AND velocity to stream them to my robot controller (which supports joint trajectory streaming consisting in timestamed positon/velocity points). There are some compute_ik and compute_fk services in move_group but they seem to be only performing position ik which does not seem sufficient to me. Is there a way to get a handle to the solver and use it in another node? Would it be a good practice or is there a better way (adding a service to move_group, etc.)?

3- I am confused about how rviz shows 3 versions of the same robot but there is only one tree in tf. If I want to show the previous IK solution in rviz, how should I do it?

Thanks

[Ioan Sucan]

Hello Flavian,

I will try to answer your questions inline:

On Tue, Sep 3, 2013 at 12:16 PM, Flavian Hautbois <f.hau...@gmail.com> wrote:

Hello everyone,

I have been using MoveIt with the Motoman SIA 10D for a few days, it works great! Congratulation to the people from I-ROS and MoveIt.
However my main application was not doing path planning, but "real-time" teleoperation (best effort actually, I'd like to achieve ~50Hz).
This is my setup: I launch the MoveIt application with the controller for my robot (launch file enclosed) and I have a node that publishes a cartesian goal (position + speed).

I have several questions:

1- what message type should I use for the cartesian goal, and were should I publish it? (I was thinking of publishing it as a child to base_link in tf)

I am not sure I understand what you would like to do. Are you trying to compute a plan to that goal?

If so, you should use the MotionPlanRequest message, or use the planning interface  library to interface with planning:
http://moveit.ros.org/wiki/MoveGroup_Interface_tutorial

2- I am using the constraint aware kinematics solver to compute joint trajectories ;

I am not sure what that is. There is no constraint aware kinematics solver in MoveIt.
 

I want to compute IK position AND velocity to stream them to my robot controller (which supports joint trajectory streaming consisting in timestamed positon/velocity points). There are some compute_ik and compute_fk services in move_group but they seem to be only performing position ik which does not seem sufficient to me. Is there a way to get a handle to the solver and use it in another node? Would it be a good practice or is there a better way (adding a service to move_group, etc.)?

What you could do is compute position IK, either using setFromIK on a RobotState, or using computeCartesianPath() from the RobotState. ROS services  also exist for move_group.

Then you can parametrize the velocity with time stamps using the trajectory_processing lib.

3- I am confused about how rviz shows 3 versions of the same robot but there is only one tree in tf. If I want to show the previous IK solution in rviz, how should I do it?

You see the current state of the robot (as set in the planning scene, not necessarily that of the system), the start state, and the goal state.

Hope this helps,

Ioan
 

Thanks

[Flavian Hautbois]

Hello Ioan,

Thanks for the quick response.  The main point here is that I want to bypass any motion planning ability in MoveIt and simply stream direct joint positions/velocities. But I need some IK at some point, that the MoveIt motion planning has to use at some point to compute a trajectory.
I actually meant collision aware IK solver and not constraint aware (maybe it's the same).

Flavian

[Ioan Sucan]

On Tue, Sep 3, 2013 at 12:55 PM, Flavian Hautbois <f.hau...@gmail.com> wrote:

Hello Ioan,

Thanks for the quick response.  The main point here is that I want to bypass any motion planning ability in MoveIt and simply stream direct joint positions/velocities. But I need some IK at some point, that the MoveIt motion planning has to use at some point to compute a trajectory.
I actually meant collision aware IK solver and not constraint aware (maybe it's the same).

Still not sure what you mean. Do you mean that you pass in a constraint validation routine for IK?

I think you should use the compute_cartesian_path service provided by move_group; That computes IK, constructs a trajectory and does not hit obstacles if possible  (if you ask for that). There is no call to a planner from OMPL.

[Adolfo Rodríguez Tsouroukdissian]

On Tue, Sep 3, 2013 at 12:00 PM, Ioan Sucan <isu...@gmail.com> wrote:

On Tue, Sep 3, 2013 at 12:55 PM, Flavian Hautbois <f.hau...@gmail.com> wrote:

Hello Ioan,

Thanks for the quick response.  The main point here is that I want to bypass any motion planning ability in MoveIt and simply stream direct joint positions/velocities. But I need some IK at some point, that the MoveIt motion planning has to use at some point to compute a trajectory.
I actually meant collision aware IK solver and not constraint aware (maybe it's the same).

Still not sure what you mean. Do you mean that you pass in a constraint validation routine for IK?

I think you should use the compute_cartesian_path service provided by move_group; That computes IK, constructs a trajectory and does not hit obstacles if possible  (if you ask for that). There is no call to a planner from OMPL.

Hey Ioan, just curious here, how does it try not to avoid obstacles?, is there some form of local collision avoidance in place?.

Adolfo.

[Flavian Hautbois]

Le mardi 3 septembre 2013 12:00:31 UTC+2, Ioan Alexandru Sucan a écrit :

On Tue, Sep 3, 2013 at 12:55 PM, Flavian Hautbois <f.hau...@gmail.com> wrote:

Hello Ioan,

Thanks for the quick response.  The main point here is that I want to bypass any motion planning ability in MoveIt and simply stream direct joint positions/velocities. But I need some IK at some point, that the MoveIt motion planning has to use at some point to compute a trajectory.
I actually meant collision aware IK solver and not constraint aware (maybe it's the same).

Still not sure what you mean. Do you mean that you pass in a constraint validation routine for IK?

I think you should use the compute_cartesian_path service provided by move_group; That computes IK, constructs a trajectory and does not hit obstacles if possible  (if you ask for that). There is no call to a planner from OMPL.

The only constraint is the presence of obstacle so I guess I meant collision avoidance ;).
So I guess you're right with the compute_cartesian_path service, I will use that. However it doesn't completely address my need as I can't really stream positions with it
 

[Ioan Sucan]

That is true. You would need to pass the result to the execute_trajectory service.
However, this is not true streaming. To do that, you would actually have to use the robot_state class directly and call setFromIK as needed, and then use the trajectory_execution_manager and pass points directly.
http://moveit.ros.org/hydro/doxygen/classrobot__state_1_1RobotState.html
http://moveit.ros.org/hydro/doxygen/classtrajectory__execution__manager_1_1TrajectoryExecutionManager.html

Ioan

[jz...@swri.org]

Flavian,

 As far as I know, ROS and MoveIt do not generally use cartesian velocity control as you are describing.  Instead, they do position-based control to calculate a joint-space trajectory (series of joint positions), and then calculate velocities, accelerations, and timing for each point to achieve smooth motion based on the robot’s joint vel/accel limits.  I don’t think cartesian velocities are ever considered.

 If you would like to generate individual joint pos/vel commands from your input cartesian pos/vel control data, then you’ll need to solve for both pieces independently:

  1) The cart->joint position can be solved using one of the IK services, as you’ve found.

  2) The cart->joint velocity requires a different method.  Here are two options:

      a) Approximate Derivatives: choose a small time step, and calculate a “future” cartesian position based on the commanded cartesian position and velocity.  Use IK to calculate joint positions for both the commanded cartesian position and the “future” position.  Take the difference between the current/future joint positions and divide by your timestep to approximate the joint-space velocities corresponding to your cartesian velocity.

    b) Jacobian: the manipulator Jacobian relates cartesian-space velocity “twist” vector to joint-space velocities, as described here.  Calculate a jacobian for the current robot position. Convert your cartesian velocities into a “twist” vector, such that rotational velocities are expressed as velocities about the coordinate axes (not necessarily Roll/Pitch/Yaw).  Solve the equation cart_vel = Jacobian*joint_vel.  For simple (6DOF, non-singular) configurations, this is simply joint_vel = inv(Jacobian) * cart_vel.  For your 7-DOF robot, you’ll need to solve an under-constrained matrix problem.  The Eigen matrix lib is used by ROS and has functionality to do this.

 Before you pursue either of these approaches, make sure that this is the correct approach for your problem.  There’s some redundant information in the position/velocity/accel/time-from-start fields when generating your trajectories.  It’s easy to create trajectories that are “disjoint” between different trajectory segments, resulting in “noisy” motion of the robot.  That’s part of the reason why ROS tends to process all generated trajectories through a smoothing filter prior to execution on the robot.  You may need to provide a similar on-the-fly smoothing filter in your controller, to get the motion you’re looking for.  Consider whether you need to apply this filter in cartesian space, or if it’s just as easy to do the filtering in joint-space, bypassing the cart->joint velocity conversion you’re stuck on.

 Hope this helps!
  Jeremy Zoss
  Southwest Research Institute