def smoothPlan(self, transforms):
"""
Smooth plan function which solves for all IK solutions for each transform given.
It then chooses the joint DOF values which are closest to current DOF values.
List of transforms along path -> transforms
"""
if len(transforms) < 1:
rospy.ERROR('Not enough transforms!')
if len(transforms) == 1:
initTransform = self.arm.manip.GetEndEffectorTransform()
transforms = [initTransform, transforms[0]]
trajectory = []
armIndices = self.arm.manip.GetArmIndices()
currJoints = self.arm.manip.GetRobot().GetDOFValues(armIndices)
# currJoints = self.arm.get_joint_positions()
for transform in transforms:
allJoints = self.arm.manip.FindIKSolutions(transform, self.filter_options)
if not allJoints.size:
rospy.logwarn('IK Failed on ' + self.rl_long + 'arm.')
return None
allJoints = [ku.closer_joint_angles(joints, currJoints) for joints in allJoints]
normDifferences = [norm(currJoints-joints,2) for joints in allJoints]
minIndex = normDifferences.index(min(normDifferences))
trajectory.append(allJoints[minIndex])
return trajectory
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)