def NumpyArraytoPyKDLFrame(self, pos, rot, unit='rad'):
if unit == 'deg':
rot = U.deg_to_rad(rot)
px, py, pz = pos
rz, ry, rx = np.array([-np.pi / 2, np.pi, 0]) - np.array(rot)
return PyKDL.Frame(PyKDL.Rotation.EulerZYX(rz, ry, rx),
PyKDL.Vector(px, py, pz))
def set_pose(self, pos, rot, unit='rad', wait_callback=True):
"""
:param pos_des: position array [x,y,z]
:param rot_des: rotation array [Z,Y,X euler angle]
:param unit: 'rad' or 'deg'
:param wait_callback: True or False
"""
if unit == 'deg':
rot = U.deg_to_rad(rot)
# limiting range of motion
limit_angle = 89 * np.pi / 180
if rot[1] > limit_angle: rot[1] = limit_angle
elif rot[1] < -limit_angle: rot[1] = -limit_angle
if rot[2] > limit_angle: rot[2] = limit_angle
elif rot[2] < -limit_angle: rot[2] = -limit_angle
# set in position cartesian mode
frame = self.NumpyArraytoPyKDLFrame(pos, rot)
msg = posemath.toMsg(frame)
print msg
# go to that position by goal
if wait_callback:
return self.__set_position_goal_cartesian_publish_and_wait(msg)
else:
self.goal_reached = False
self.__set_position_goal_cartesian_pub.publish(msg)
return True
def set_jaw_direct(self, jaw, unit='rad'):
"""
:param jaw: jaw angle
:param unit: 'rad' or 'deg'
"""
if unit == 'deg':
jaw = U.deg_to_rad(jaw)
msg = JointState()
msg.position = [jaw]
self.__set_position_jaw_pub.publish(msg)
def set_jaw(self, jaw, unit='rad', wait_callback=True):
"""
:param jaw: jaw angle
:param unit: 'rad' or 'deg'
:param wait_callback: True or False
"""
if unit == 'deg':
jaw = U.deg_to_rad(jaw)
msg = JointState()
msg.position = [jaw]
if wait_callback:
return self.__set_position_goal_jaw_publish_and_wait(msg)
else:
self.__set_position_goal_jaw_pub.publish(msg)
return True
def set_joint(self, joint, unit='rad', wait_callback=True):
"""
:param joint: joint array [j1, ..., j6]
:param unit: 'rad', or 'deg'
:param wait_callback: True or False
"""
if unit == 'deg':
joint = U.deg_to_rad(joint)
msg = JointState()
msg.position = joint
if wait_callback:
return self.__set_position_goal_joint_publish_and_wait(msg)
else:
self.__set_position_goal_joint_pub.publish(msg)
return True
def set_pose_direct(self, pos, rot, unit='rad'):
"""
:param pos_des: position array [x,y,z]
:param rot_des: rotation array [Z,Y,X euler angle]
:param unit: 'rad' or 'deg'
"""
if unit == 'deg':
rot = U.deg_to_rad(rot)
# limiting range of motion
limit_angle = 89 * np.pi / 180
if rot[1] > limit_angle: rot[1] = limit_angle
elif rot[1] < -limit_angle: rot[1] = -limit_angle
if rot[2] > limit_angle: rot[2] = limit_angle
elif rot[2] < -limit_angle: rot[2] = -limit_angle
# set in position cartesian mode
frame = self.NumpyArraytoPyKDLFrame(pos, rot)
msg = posemath.toMsg(frame)
# go to that position by goal
self.__set_position_cartesian_pub.publish(msg)
def ZYXtoquaternion(self, rot, unit='rad'):
if unit == 'deg':
rot = U.deg_to_rad(rot)
rz, ry, rx = np.array([-np.pi / 2, np.pi, 0]) - np.array(rot)
R = PyKDL.Rotation.EulerZYX(rz, ry, rx)
return R.GetQuaternion()
