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