how to open pr2 gripper with moveit_commander
Hi, I cannot get move group to open gripper of pr2 eventhough I can successfully move the head torso and other parts of pr2 using the MoveGroupCommander. See the code below with two basic functions: move_head and open_gripper. This does not work when I try to open the gripper's hand. Note that the gripper's hand closes when it is opened and I set all the joint values to zero. But I can't get it to open when the gripper's hands are closed.
#!/usr/bin/python
# coding: utf-8
import sys
import moveit_commander
import rospy
import roscpp
def home_head():
mgc = moveit_commander.MoveGroupCommander("head")
jv = mgc.get_current_joint_values()
jv[0] = 0.0
jv[1] = 0.5
mgc.set_joint_value_target(jv)
p = mgc.plan()
mgc.execute(p)
def open_right_hand():
mgc = moveit_commander.MoveGroupCommander("right_gripper")
jv = mgc.get_current_joint_values()
jv = [1.0, 1.0, 0.477, 0.477, 0.477, 0.477, 0.04]
mgc.set_joint_value_target(jv)
p = mgc.plan()
mgc.execute(p)
if __name__ == "__main__":
node_name = "ready_pr2"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node(node_name)
rospy.loginfo(node_name + ": is initialized")
rc = moveit_commander.RobotCommander()
rospy.loginfo("robot commander is initialized")
home_head()
rospy.loginfo("head homed")
open_right_hand()
rospy.loginfo("right_gripper opened")
moveit_commander.roscpp_shutdown()
Hi, did you resolved your issue ? I am also trying to move the gripper with moveit at a joint level. I can control the gripper with GripperCommand value (position, max effort) but can't manage to control it with joint. Joint control of gripper is necessary to be able to use Pick place pipeline but also move it task constructor which make grasping easier.