def close_l_gripper(self):
sjs = SetJointStateRequest()
sjs.state.name = [
u'l_gripper_l_finger_joint', u'l_gripper_r_finger_joint',
u'l_gripper_l_finger_tip_joint', u'l_gripper_r_finger_tip_joint'
]
sjs.state.position = [0, 0, 0, 0]
self.r_gripper.call(sjs)
def teleport_base(self, goal_pose):
goal_pose = transform_pose(self.default_root, goal_pose)
js = {
u'odom_x_joint':
goal_pose.pose.position.x,
u'odom_y_joint':
goal_pose.pose.position.y,
u'odom_z_joint':
rotation_from_matrix(
quaternion_matrix([
goal_pose.pose.orientation.x, goal_pose.pose.orientation.y,
goal_pose.pose.orientation.z, goal_pose.pose.orientation.w
]))[0]
}
goal = SetJointStateRequest()
goal.state = dict_to_joint_states(js)
self.set_base.call(goal)
def open_r_gripper(self):
sjs = SetJointStateRequest()
sjs.state.name = [
u'r_gripper_l_finger_joint', u'r_gripper_r_finger_joint',
u'r_gripper_l_finger_tip_joint', u'r_gripper_r_finger_tip_joint'
]
sjs.state.position = [0.54, 0.54, 0.54, 0.54]
self.r_gripper.call(sjs)
def open_l_gripper(self):
sjs = SetJointStateRequest()
sjs.state.name = [
u'l_gripper_l_finger_joint', u'l_gripper_r_finger_joint',
u'l_gripper_l_finger_tip_joint', u'l_gripper_r_finger_tip_joint'
]
sjs.state.position = [0.54, 0.54, 0.54, 0.54]
sjs.state.velocity = [0, 0, 0, 0]
sjs.state.effort = [0, 0, 0, 0]
self.l_gripper.call(sjs)
def mvt_r_gripper(self, l_finger_joint, r_finger_joint, l_finger_tip_joint, r_finger_tip_joint):
"""
execute mvt of right gripper
:param l_finger_joint: value of r_gripper_l_finger_joint
:type: float
:param r_finger_joint: value of r_gripper_r_finger_joint
:type: float
:param l_finger_tip_joint: value of r_gripper_l_finger_tip_joint
:type: float
:param r_finger_tip_joint: value of r_gripper_r_finger_tip_joint
:type: float
:return:
"""
rospy.logout('Start mvt R gripper')
gripper_request = SetJointStateRequest()
gripper_request.state.name = ['r_gripper_l_finger_joint', 'r_gripper_r_finger_joint',
'r_gripper_l_finger_tip_joint', 'r_gripper_r_finger_tip_joint']
gripper_request.state.position = [l_finger_joint, r_finger_joint, l_finger_tip_joint, r_finger_tip_joint]
gripper_request.state.velocity = [0, 0, 0, 0]
gripper_request.state.effort = [0, 0, 0, 0]
self.r_gripper_service.call(gripper_request)
rospy.logout('End mvt R gripper')