Hi All. I am trying to find out the Inverse Kinematic solution for the Atlas Arm. I am trying to use the 'arm_kinematics' package for this. The steps I used for this are as follows:
I entered the following commands in seperate terminals:
roslaunch urdf_tutorial display.launch model:=atlas_sandia_hands.urdf
roslaunch atlas_utils atlas_sandia_hands.launch
In a separate terminal I entered the following commands:
- rosparam set urdf_xml robot_description
- rosparam set robot_description atlas_sandia_hands.urdf
- rosparam set arm_kinematics/root_name utorso
- rosparam set arm_kinematics/tip_name l_hand
- rosparam set maxIterations 1000
- rosrun arm_kinematics arm_kinematics
Then I wrote a python script for finding out the IK solution. I tried following the tutorial for PR2 arm inverse kinematics. This is what I had:
! /usr/bin/env python
import roslib; roslib.load_manifest('tutorial_atlas_control') roslib.load_manifest('geometry_msgs') roslib.load_manifest('kinematics_msgs')
import rospy, yaml, sys from atlas_msgs.msg import AtlasCommand from sensor_msgs.msg import JointState from std_msgs.msg import * from geometry_msgs.msg import * from kinematics_msgs.srv import * from arm_navigation_msgs.srv import *
class findik:
def get_ik(self, x, y, z):
rospy.init_node('find_ik')
self.ik_info = '/arm_kinematics/get_ik_solver_info'
print "waiting for ik service"
rospy.wait_for_service(self.ik_info)
self.ik_info_service = rospy.ServiceProxy(self.ik_info, GetKinematicSolverInfo)
self.ik_info_service.joint_names = ["l_arm_usy",
"l_arm_shx",
"l_arm_ely",
"l_arm_elx",
"l_arm_uwy",
"l_arm_mwx"]
self.ik_info_service.joint_names = ["utorso",
"l_clav",
"l_scap",
"l_uarm",
"l_larm",
"l_farm",
"l_hand"]
self.ik_info_req = GetKinematicSolverInfoRequest()
self.ik_info_res = self.ik_info_service.call(self.ik_info_req)
self.ik_service = rospy.ServiceProxy('arm_kinematics/get_ik', GetPositionIK, True)
self.ik_req = GetPositionIKRequest()
self.ik_req.timeout = rospy.Duration(5.0)
self.ik_req.ik_request.ik_link_name = 'l_hand'
self.ik_req.ik_request.ik_seed_state.joint_state.name = self.ik_info_res.kinematic_solver_info.joint_names
self.ik_req.ik_request.ik_seed_state.joint_state.position = [0]*6
self.ps = PoseStamped()
self.ps.header.stamp = rospy.Time(0)
self.ps.header.frame_id = 'pelvis'
self.ps.pose.position.x = x
self.ps.pose.position.y = y
self.ps.pose.position.z = z
self.ps.pose.orientation.x = 0
self.ps.pose.orientation.y = 0
self.ps.pose.orientation.z = 0
self.ps.pose.orientation.w = 1.0
self.ik_req.ik_request.pose_stamped = self.ps
self.ik_res = self.ik_service.call(self.ik_req)
if __name__ == '__main__': ik = findik() ik.get_ik(0.55, -0.15, 1.2)
However the command "self.ik_res = self.ik_service.call(self.ik_req)" would result in rospy exception and would stop the "arm_kinematics" node in the other terminal.
Has any one tried finding IK for Atlas. What Package did you use. If you used "arm_kinematics" did you run into similar problem?? Please let me know. I am using ros Fuerte, Gazebo 1.8.2 and drcsim 2.6.6.1 in ubuntu 12.04.
Any help will be highly appreciated. Thanks.
