Hello
I am currently trying to grasp several small boxes with the youbot. I have set it up with MoveIt, but I cannot figure out how to give it valid a valid pose. I am not concerning myself with proper object recognition(via sensors), so I am trying to get the object position from Gazebo and give it to MoveIt to move the arm next to the box. Currently I am getting the position of the box using the GetModelState service call. But if I feed the pose it returns me to moveit it once again tells me that I have specified a wrong pose. Here is the code:
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
arm = moveit_commander.MoveGroupCommander("arm")
def listener():
rospy.init_node('listener',anonymous=True)
rospy.wait_for_service("/gazebo/get_model_state")
try:
gms = rospy.ServiceProxy("/gazebo/get_model_state",GetModelState)
response = gms("box","youbot::arm_link_5")
youbot_node.plan(response)
except rospy.ServiceException, e:
print "Service call failsed: %s"%e
def plan(targetPose):
pose_target = geometry_msgs.msg.PoseStamped()
pose_target.header.stamp = rospy.Time.now()
pose_target.header.frame_id = "/odom"
pose_target.pose = targetPose.pose
print "============================="
print pose_target
arm.go(pose_target)
So any idea what I should do to move my arm to the proper position, and then grasp the box?
↧