Unable to set solver_type in launch file via kinematics.yaml or param/rosparam

[Christopher Mobley]

To Whom It May Concern,

Thank you very much. This package is great. I tried to use ur_kinematics for IK with my UR5 unsuccessfully before finding this package. I just downloaded the .deb, changed the kinematics.yaml file and everything worked as planned.

However, when I use the code below:

# Give the node a name
rospy.init_node("drill_prediction", anonymous=False)
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the move group for the arm
ur5_arm = moveit_commander.MoveGroupCommander("ur5_arm")
# Get the name of the end-effector link
end_effector_link = ur5_arm.get_end_effector_link()
# Set the reference frame for pose targets
 reference_frame = "/base_link"
# Set the arm reference frame accordingly
ur5_arm.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
ur5_arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
ur5_arm.set_goal_position_tolerance(0.01)
ur5_arm.set_goal_orientation_tolerance(0.05)
#Move the end effecor to the x , y, z positon
#Set the target pose in the base_link frame
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 1.0 #hole_pose.position.x
target_pose.pose.position.y = .1 #hole_pose.position.y
target_pose.pose.position.z = .7 #hole_pose.position.z
target_pose.pose.orientation.x = 0 #hole_pose.orientation.x
target_pose.pose.orientation.y = 0 #hole_pose.orientation.y
target_pose.pose.orientation.z = 0 #hole_pose.orientation.z
target_pose.pose.orientation.w = 1 #hole_pose.orientation.w
# Set the start state to the current state
ur5_arm.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
ur5_arm.set_pose_target(target_pose, end_effector_link)
# Plan the trajectory to the goal
traj = ur5_arm.plan()
# Execute the planned trajectory
ur5_arm.execute(traj)
# Pause for a second
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)

The nodes fails to set the params as I have set them in my kinematics.yaml

ur5_arm:
  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  kinematics_solver_timeout: 0.05
  solve_type: Manipulation1

and returns this

[ INFO] [1467410538.629341568, 298.510000000]: Looking in private handle: /move_group_commander_wrappers_1467410538042921053 for param name: ur5_arm/position_only_ik
[ INFO] [1467410538.629825732, 298.510000000]: Looking in private handle: /move_group_commander_wrappers_1467410538042921053 for param name: ur5_arm/solve_type
[ INFO] [1467410538.630190163, 298.510000000]: Using solve type Speed

Despite my launch file below including every way I can think of to set these parameters.

<launch>

  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
  <include file="$(find husky_ur5_moveit_config)/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
  </include>

  <node name="drill_predition" pkg="ccam_arm_nav" type="drill_prediction.py" output="screen">
    <rosparam command="load" file="$(find husky_ur5_moveit_config)/config/kinematics.yaml"/>
    <param name="arm_prefix" value="ur5_arm_"/> 
    <param name="move_group/ur5_arm/solve_type" value="Manipulation1"/>
    <param name="move_group/ur5_arm/kinematics_solver_timeout" value="0.05"/>
    <param name="ur5_arm/solve_type" value="Manipulation1"/>
    <param name="ur5_arm/kinematics_solver_timeout" value="0.05"/>
    <param name="solve_type" value="Manipulation1"/>
    <param name="kinematics_solver_timeout" value="0.05"/>
    <rosparam>
      move_group/ur5_arm/solve_type: 'Manipulation1'
      move_group/ur5_arm/kinematics_solver_timeout: 0.05
      ur5_arm/solve_type: 'Manipulation1'
      ur5_arm/kinematics_solver_timeout: 0.05
      solve_type: 'Manipulation1'
      kinematics_solver_timeout: 0.05
    </rosparam>
  </node>

</launch>

However, move_group.launch and moveit_rviz.launch seem to set them correctly. Accuracy rather than speed is my desire; so, I like to test out the other solver_types. Any help you are able to give would be greatly appreciated.

Thank again. Have a great weekend. God bless.

Best, Cmobley7

[Patrick Beeson]

This certainly sounds like that parameter setting got broken at some point. I will investigate within 24 hours.

[Patrick Beeson]

This is not a TRAC-ik plugin issue, but an issue that affects all MoveIt! kinematics plugins (including the default KDL one).

The issue here is that my plugin (just like the KDL one) relies on MoveIt! to pass in the node name so that it can read private params. In the typical ur5_moveit_config demo.launch case, if you run rosparam list | grep kinematics you see this:

$ rosparam list | grep kinematics
/robot_description_kinematics/manipulator/kinematics_solver
/robot_description_kinematics/manipulator/kinematics_solver_timeout
/robot_description_kinematics/manipulator/solve_type
/rviz_tardis_31297_1193888744139614556/manipulator/kinematics_solver
/rviz_tardis_31297_1193888744139614556/manipulator/kinematics_solver_timeout

But the actual nodes list shows this:

 $ rosnode list
/joint_state_publisher
/move_group
/robot_state_publisher
/rosout
/rviz_tardis_31297_1193888744139614556

So, as one would expect, the Rviz node actually does say Solve Type Manipulation1 in the info spew (or whatever type you set it to) because it reads the params correctly. But the robot_description_kinematics parameter space does not correspond to any ros node. In my example (from the ur5_moveit_config demo.launch), everything works once I add the following somewhere before move_group gets launched:

  <group ns="move_group">
    <rosparam command="load" file="$(find ur5_moveit_config)/config/kinematics.yaml"/>
  </group>

In your case, you'll need to add such a block so that it corresponds to your ros node.

[Patrick Beeson]

I never fully understood why MoveIt! didn't just use a global get of params given this exact scenario tripped me up a year ago, and seems to be a constant source of confusion. Maybe @davetcoleman has insights on a better solution?

[Dave Coleman]

As I recall (without digging into the code again to check this statement) the global set of params is robot_description_kinematics. For each node that uses MoveIt! kinematics, MoveIt! should first look in the local namespace for kinematics settings and if none are specified on the parameter server, it falls back to robot_description_kinematics settings. But it sounds like this isn't working for you for some reason...

[Patrick Beeson]

That IS NOT what the KDL plugin does, and thus not what IKFast not TRAC-IK plugin do. Because /robot_description_kinematics is defined by the $(arg robot_description) variable in the launch file, it could be anything. And the plugin code has no clue about this variable because its only defined the the launch files, not in any ROS parameter.

[Patrick Beeson]

Marking this resolved since there is a launch file fix.

[Christopher Mobley]

@pbeeson and @davetcoleman,

My apologies for the delay in responding to this issue. However, I added

  <group ns="move_group">
    <rosparam command="load" file="$(find ur5_moveit_config)/config/kinematics.yaml"/>
  </group>

to my move_group.launch file before starting this issue. While this allowed move_group and rviz to set the correct solve type. Python's moveit_commander still initializes with the incorrect solve_type. Do you have any idea what namespace I should add the kinematics.yaml to in order to correct this issue?

Thanks for all your help. Have a great weekend. God bless.

Very Respectfully, Christopher J. Mobley

[Abishek Hariharan]

Is there some way to set the Solve Type or configure the IK programmatically via one of the moveit classes ?

[PDMIKE]

@abishekh, @pbeeson, @CMobley7

Hi, I have added

<group ns="move_group">
    <rosparam command="load" file="$(find MY_PACKAGE)/config/kinematics.yaml"/>
  </group>

to the move_group.launch and when the package is launched, the parameter , solve_type, is correctly loaded but
do you have any idea about how to avoid the solve_type being set to ’speed‘ when running the C++ code ?