def get_current_xyz(self):
"""Get x,y,z coordinates according to currrent joint angles
Returns:
xyz are the x,y,z coordinates of an end-effector in a Cartesian space.
"""
'''
rostopic echo /joint_states
name:
- elbow_joint
- gripper_finger1_joint
- shoulder_lift_joint
- shoulder_pan_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
'''
joint_states = self.joints_state
shp_joint_ang = joint_states.position[3]
shl_joint_ang = joint_states.position[2]
elb_joint_ang = joint_states.position[0]
wr1_joint_ang = joint_states.position[4]
wr2_joint_ang = joint_states.position[5]
wr3_joint_ang = joint_states.position[6]
q = [
shp_joint_ang, shl_joint_ang, elb_joint_ang, wr1_joint_ang,
wr2_joint_ang, wr3_joint_ang
]
mat = ur_utils.forward(q, self._ik_params)
xyz = mat[:3, 3]
return xyz
def get_xyz(self, q):
"""Get x,y,z coordinates
Args:
q: a numpy array of joints angle positions.
Returns:
xyz are the x,y,z coordinates of an end-effector in a Cartesian space.
"""
mat = ur_utils.forward(q, self._ik_params)
xyz = mat[:3, 3]
return xyz
def get_orientation(self, q):
"""Get Euler angles
Args:
q: a numpy array of joints angle positions.
Returns:
xyz are the x,y,z coordinates of an end-effector in a Cartesian space.
"""
mat = ur_utils.forward(q, self._ik_params)
orientation = mat[0:3, 0:3]
roll = -orientation[1, 2]
pitch = orientation[0, 2]
yaw = -orientation[0, 1]
return Vector3(roll, pitch, yaw)
def get_current_xyz(self):
"""Get x,y,z coordinates according to currrent joint angles
Returns:
xyz are the x,y,z coordinates of an end-effector in a Cartesian space.
"""
joint_states = self.joints_state
shp_joint_ang = joint_states.position[0]
shl_joint_ang = joint_states.position[1]
elb_joint_ang = joint_states.position[2]
wr1_joint_ang = joint_states.position[3]
wr2_joint_ang = joint_states.position[4]
wr3_joint_ang = joint_states.position[5]
q = [shp_joint_ang, shl_joint_ang, elb_joint_ang, wr1_joint_ang, wr2_joint_ang, wr3_joint_ang]
mat = ur_utils.forward(q, self._ik_params)
xyz = mat[:3, 3]
return xyz