Cartesian controller for ROS

edit

In ROS, the task of converting Cartesian goal positions into a stream of joint positions is traditionally described as the "planning" step. This task typically involves not only IK, but also collision detection and joint-space search algorithms to identify a valid trajectory between the "current" joint state and the specified goal state. Additionally, this planning step may include additional constraints (e.g. maintain desired end-effector orientation) and various trajectory-smoothing filters.

As a first step, you'll need to describe how much of this planning capability you want/need to utilize. I will outline 3 potential approaches below. I recommend #2, but have never used this method. In the past, I have used #1, but that approach requires you to manually provide some of the functionality that MoveIt does automatically in method #2.

1) Direct IK Solution

In this approach, you will run IK to calculate a joint position for each cartesian point. These joint points will be combined into a single trajectory and sent to the robot. The trajectory may require additional code support to be robust: velocity/accel smoothing, arm-configuration continuity, etc.

• create a JointTrajectoryActionGoal message to hold the joint trajectory
• create a MoveIt JointStateGroup to use for calculating IK (as shown here)
• for each cartesian point:
  • call joint_state_group->setFromIK() to calc IK
    • NOTE: this can take an Eigen::Affine3d input
    • NOTE: may need to call joint_state_group->setVariableValues() to set initial joint state
  • call joint_state_group->getVariableValues() to get computed joint position
  • create a JointTrajectoryPoint from your joint position
    • NOTE: you'll need to compute the appropriate velocity/accel values yourself
  • add JointTrajectoryPoint to FollowJointTrajectoryActionGoal.goal.trajectory.points*
• send the completed joint trajectory (FollowJointTrajectoryActionGoal) to the appropriate action. For ROS-I nodes, this is typically "joint_trajectory_action".
• When you start the motoman ROS-I nodes, it typically brings up both the interface to the actual robot and a joint_trajectory_action node that manages sending the FollowJointTrajectoryActionGoal to the robot interface nodes.
  • check to make sure that action node is running: rosnode info joint_trajectory_action

2) MoveIt ComputeCartesianPath

In this approach, you pass the full sequence of cartesian points to MoveIt. MoveIt computes the full joint trajectory between points, while also providing additional functionality beyond the basic approach described above: It inserts additional points to maintain linear motion, automatically checks for collisions and arm-configuration "flips", and generates the velocities and accelerations needed for smooth motion.

• create a MoveIt MoveGroupInterface object, as shown here
• call move_group->ComputeCartesianPath() with your sequence of cartesian points
  • NOTE: this function accepts a std::vector<Pose>, so you'll need to convert your cartesian positions using tf::poseEigenToMsg()
  • this returns a RobotTrajectory.
• send your trajectory to MoveIt
  • the easiest way I've found is to use the move_group node's ExecuteKnownTrajectory service, which is available on my setup at "/execute_kinematic_path".
  • alternatively, you could probably send it to the TrajectoryExecutionManager object directly, as shown here, but that seems messy

3) MoveIt Point-to-Point

This approach is probably the "easiest", but is unlikely to give you the motion you are looking for. You pass individual ... (more)