def cart_to_joint(manip, matrix4, ref_frame, targ_frame, filter_options = 0): robot = manip.GetRobot() worldFromEE = transform_relative_pose_for_ik(manip, matrix4, ref_frame, targ_frame) joint_positions = manip.FindIKSolution(worldFromEE, filter_options) if joint_positions is None: return joint_positions current_joints = robot.GetDOFValues(manip.GetArmIndices()) joint_positions_unrolled = ku.closer_joint_angles(joint_positions, current_joints) return joint_positions_unrolled
def cart_to_joint(manip, matrix4, ref_frame, targ_frame, filter_options=0): robot = manip.GetRobot() worldFromEE = transform_relative_pose_for_ik(manip, matrix4, ref_frame, targ_frame) joint_positions = manip.FindIKSolution(worldFromEE, filter_options) if joint_positions is None: return joint_positions current_joints = robot.GetDOFValues(manip.GetArmIndices()) joint_positions_unrolled = ku.closer_joint_angles(joint_positions, current_joints) return joint_positions_unrolled
def goto_joint_positions(self, positions_goal): positions_cur = self.get_joint_positions() positions_goal = ku.closer_joint_angles(positions_goal, positions_cur) TrajectoryControllerWrapper.goto_joint_positions(self, positions_goal)