Define correct move_group for using TracIK with python

asked 2016-11-24 08:33:46 -0600

florian_v gravatar image

updated 2016-11-24 08:35:10 -0600

Hello,

when I try to interact with a move_group via a python script called by a launch file, the default value for solve_type is used instead of the one defined in the kinematics.yaml.

This is what my kinematics.yaml file looks like:

arm:    
kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin 
kinematics_solver_timeout: 0.005    
solve_type: Manipulation1

When I start the robot system and moveit with a separate launch file, it seems to read the content of kinematics.yaml correctly:

PARAMETERS
...
 * /robot_description_kinematics/arm/kinematics_solver: trac_ik_kinematic...
 * /robot_description_kinematics/arm/kinematics_solver_timeout: 0.005
 * /robot_description_kinematics/arm/solve_type: Manipulation1
 ...

This is my script:

#!/usr/bin/env python
# import libs
# Initilaize

moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
group = moveit_commander.MoveGroupCommander("arm")

joint_pub_arm = rospy.Publisher('robot_arm/arm/joint_trajectory_controller/command',JointTrajectory , queue_size=1)
joint_pub_base= rospy.Publisher('robot_arm/base/joint_trajectory_controller/command',JointTrajectory , queue_size=1)

def callback(data_in): 

    #generate pose ...

    plan1= group.plan(p.pose)

    msg = plan1.joint_trajectory

    comb_msg = split_msg_base_arm (msg)
    base_msg = comb_msg[0]
    arm_msg = comb_msg[1]

    joint_pub_arm.publish(arm_msg)
    joint_pub_base.publish(base_msg)

# ============================================================================================
def listener(): 
    rospy.init_node("touch_listener", anonymous=True)
    rospy.Subscriber("touchscreen", touchscreen_data,  callback, queue_size=1)   
    rospy.spin()

if __name__ == "__main__":
    listener()

This is my launch file:

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

  <node name="touch_listener" pkg="demo_1" type="fast_hack_touch_listener.py" respawn="false" output="screen">
  </node>  
</launch>

When i try to launch it the following happens:

started roslaunch server http://robotino:47211/

SUMMARY
========

PARAMETERS
 * /move_group/arm/kinematics_solver: trac_ik_kinematic...
 * /move_group/arm/kinematics_solver_timeout: 0.005
 * /move_group/arm/solve_type: Manipulation1
 * /rosdistro: indigo
 * /rosversion: 1.11.19

NODES
  /
    touch_listener (demo_1/fast_hack_touch_listener.py)

ROS_MASTER_URI=http://localhost:11311

core service [/rosout] found
process[touch_listener-1]: started with pid [8631]
[ INFO] [1479996271.812067758]: Loading robot model 'robotino_arm_6dof'...
[ INFO] [1479996272.162000297]: IK Using joint base_link -3.40282e+38 3.40282e+38
[ INFO] [1479996272.162113755]: IK Using joint link1 -1.8326 1.8326
[ INFO] [1479996272.162173560]: IK Using joint motor2 -2.44346 2.44346
[ INFO] [1479996272.162216309]: IK Using joint link3 -3.14159 3.14159
[ INFO] [1479996272.162276581]: IK Using joint link4 -3.14159 3.14159
[ INFO] [1479996272.162358474]: IK Using joint link5 -3.14159 3.14159
[ INFO] [1479996272.162430079]: Looking in private handle: /move_group_commander_wrappers_1479996271800514391 for param name: arm/position_only_ik
[ INFO] [1479996272.163764172]: Looking in private handle: /move_group_commander_wrappers_1479996271800514391 for param name: arm/solve_type
[ INFO] [1479996272.164922544]: Using solve type Speed
[ INFO] [1479996273.084825013]: Ready to take MoveGroup commands for group arm.

A new move_group_commander_wrapper_* is created instead of using the existing move_group to get the parameter from. I guess that this is a wrong usage of the move_group python interface.

How can I correct this?

Thank you very much!

edit retag flag offensive close merge delete