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)
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)
