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)
