Ejemplo n.º 1
0
    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
Ejemplo n.º 2
0
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
Ejemplo n.º 3
0
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
Ejemplo n.º 4
0
 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)
Ejemplo n.º 5
0
 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)