def __create_trjectory_non_collision(self, arm, positions, orientations, frame_id, max_vel, ignore_errors=False, normalize=True): if arm == "right_arm": link_name = "r_wrist_roll_link" joint_angles = self.joint_mover.robot_state.right_arm_positions ik = self.right_ik elif arm == "left_arm": link_name = "l_wrist_roll_link" joint_angles = self.joint_mover.robot_state.left_arm_positions ik = self.left_ik else: rospy.logerr("Unknown arm: %s" % arm) return False trajectory = [] for i in xrange(len(positions)): pose = ik.lists_to_pose_stamped(positions[i], orientations[i], frame_id, frame_id) (joints, e) = ik.run_ik(pose, joint_angles, link_name, collision_aware=0) if (not ignore_errors) and (e != "SUCCESS"): rospy.logerr("IK returns error codes: %s at step %d" % (str(e), i)) return False if e == "SUCCESS": trajectory.append(joints) if normalize: rospy.loginfo("Normalising trajectory") #trajectory = self.__normalize_trajectory(trajectory, joint_angles) trajectory = utils.normalize_trajectory(trajectory, joint_angles) else: rospy.loginfo("Not normalising the trajectory") (times, vels) = ik.trajectory_times_and_vels(trajectory, [max_vel] * 7) return (trajectory, times, vels)
def __create_trjectory_non_collision(self, arm, positions, orientations, frame_id, max_vel, ignore_errors = False, normalize = True): if arm == "right_arm": link_name = "r_wrist_roll_link" joint_angles = self.joint_mover.robot_state.right_arm_positions ik = self.right_ik elif arm == "left_arm": link_name = "l_wrist_roll_link" joint_angles = self.joint_mover.robot_state.left_arm_positions ik = self.left_ik else: rospy.logerr("Unknown arm: %s"%arm) return False trajectory = [] for i in xrange(len(positions)): pose = ik.lists_to_pose_stamped(positions[i], orientations[i], frame_id, frame_id) (joints,e) = ik.run_ik(pose, joint_angles, link_name, collision_aware=0) if (not ignore_errors) and (e != "SUCCESS"): rospy.logerr("IK returns error codes: %s at step %d"%(str(e),i)) return False if e == "SUCCESS": trajectory.append(joints) if normalize: rospy.loginfo("Normalising trajectory") #trajectory = self.__normalize_trajectory(trajectory, joint_angles) trajectory = utils.normalize_trajectory(trajectory, joint_angles) else: rospy.loginfo("Not normalising the trajectory") (times, vels) = ik.trajectory_times_and_vels(trajectory, [max_vel]*7) return (trajectory, times, vels)
def execute_JointTrajectory(self, joint_trajectory, normalize=True, max_vel = 0.2): """Executes a trajectory. It does not check if the trajectory is safe, nor it performs any interpolation or smoothing! If velocities or accelerations in joint_trajectory are not set then they are automatically calculated. Parameters: joint_trajectory: a JointTrajectory msg normalize: if True the continous joints are normalized max_vel = maximum velocity for all the joints """ isinstance(joint_trajectory, JointTrajectory) if joint_trajectory.joint_names[0][0] == "l": joint_angles = self.joint_mover.robot_state.right_arm_positions ik = self.right_ik else: joint_angles = self.joint_mover.robot_state.right_arm_positions ik = self.right_ik if len(joint_trajectory.points) == 0: rospy.logwarn("Empty trajectory!") return False #create velocities if they don't exist if len(joint_trajectory.points[0].velocities) == 0: trajectory = [p.positions for p in joint_trajectory.points] if normalize: #trajectory = self.__normalize_trajectory(trajectory, joint_angles) trajectory = utils.normalize_trajectory(trajectory, joint_angles) (times, vels) = ik.trajectory_times_and_vels(trajectory, [max_vel]*7) for i in xrange(len(joint_trajectory.points)): joint_trajectory.points[i].positions = trajectory[i] joint_trajectory.points[i].velocities = vels[i] joint_trajectory.points[i].time_from_start = rospy.Duration(times[i]) self.joint_mover.execute_JointTrajectory(joint_trajectory)