def interpolate_joint_space(self, goal, total_time, nb_points, start=None):
"""
Interpolate a trajectory from a start state (or current state) to a goal in joint space
:param goal: A RobotState to be used as the goal of the trajectory
param total_time: The time to execute the trajectory
:param nb_points: Number of joint-space points in the final trajectory
:param start: A RobotState to be used as the start state, joint order must be the same as the goal
:return: The corresponding RobotTrajectory
"""
dt = total_time*(1.0/nb_points)
# create the joint trajectory message
traj_msg = JointTrajectory()
rt = RobotTrajectory()
if start == None:
start = self.get_current_state()
joints = []
start_state = start.joint_state.position
goal_state = goal.joint_state.position
for j in range(len(goal_state)):
pose_lin = np.linspace(start_state[j],goal_state[j],nb_points+1)
joints.append(pose_lin[1:].tolist())
for i in range(nb_points):
point = JointTrajectoryPoint()
for j in range(len(goal_state)):
point.positions.append(joints[j][i])
# append the time from start of the position
point.time_from_start = rospy.Duration.from_sec((i+1)*dt)
# append the position to the message
traj_msg.points.append(point)
# put name of joints to be moved
traj_msg.joint_names = self.joint_names()
# send the message
rt.joint_trajectory = traj_msg
return rt
def scale_trajectory_speed(traj, scale):
# Create a new trajectory object
new_traj = RobotTrajectory()
# Initialize the new trajectory to be the same as the planned trajectory
new_traj.joint_trajectory = traj.joint_trajectory
# Get the number of joints involved
n_joints = len(traj.joint_trajectory.joint_names)
# Get the number of points on the trajectory
n_points = len(traj.joint_trajectory.points)
# Store the trajectory points
points = list(traj.joint_trajectory.points)
# Cycle through all points and scale the time from start, speed and acceleration
for i in range(n_points):
point = JointTrajectoryPoint()
point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
point.velocities = list(traj.joint_trajectory.points[i].velocities)
point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
point.positions = traj.joint_trajectory.points[i].positions
for j in range(n_joints):
point.velocities[j] = point.velocities[j] * scale
point.accelerations[j] = point.accelerations[j] * scale * scale
points[i] = point
# Assign the modified points to the new trajectory
new_traj.joint_trajectory.points = points
# Return the new trajectory
return new_traj
def dict_to_trajectory(plan):
plan_msg = RobotTrajectory()
joint_traj = JointTrajectory()
joint_traj.header.frame_id = plan["joint_trajectory"]["frame_id"]
joint_traj.joint_names = plan["joint_trajectory"]["joint_names"]
for point in plan["joint_trajectory"]["points"]:
joint_traj.points.append(
JointTrajectoryPoint(positions=point["positions"],
velocities=point["velocities"],
accelerations=point["accelerations"],
time_from_start=point["time_from_start"]))
multi_dof_joint_traj = MultiDOFJointTrajectory()
multi_dof_joint_traj.header.frame_id = plan["multi_dof_joint_trajectory"][
"frame_id"]
multi_dof_joint_traj.joint_names = plan["multi_dof_joint_trajectory"][
"joint_names"]
for point in plan["multi_dof_joint_trajectory"]["points"]:
multi_dof_joint_traj_point = MultiDOFJointTrajectoryPoint()
for t in point["transforms"]:
multi_dof_joint_traj_point.transforms.append(list_to_transform(t))
multi_dof_joint_traj_point.time_from_start = point["time_from_start"]
multi_dof_joint_traj.points.append(multi_dof_joint_traj_point)
plan_msg.joint_trajectory = joint_traj
plan_msg.multi_dof_joint_trajectory = multi_dof_joint_traj
return plan_msg
def display(self, trajectory):
"""
Display a joint-space trajectory or a robot state in RVIz loaded with the Moveit plugin
:param trajectory: a RobotTrajectory, JointTrajectory, RobotState or JointState message
"""
# Publish the DisplayTrajectory (only for trajectory preview in RViz)
# includes a convert of float durations in rospy.Duration()
def js_to_rt(js):
rt = RobotTrajectory()
rt.joint_trajectory.joint_names = js.name
rt.joint_trajectory.points.append(JointTrajectoryPoint(positions=js.position))
return rt
dt = DisplayTrajectory()
if isinstance(trajectory, RobotTrajectory):
dt.trajectory.append(trajectory)
elif isinstance(trajectory, JointTrajectory):
rt = RobotTrajectory()
rt.joint_trajectory = trajectory
dt.trajectory.append(rt)
elif isinstance(trajectory, RobotState):
dt.trajectory.append(js_to_rt(trajectory.joint_state))
elif isinstance(trajectory, JointState):
dt.trajectory.append(js_to_rt(trajectory))
else:
raise NotImplementedError("ArmCommander.display() expected type RobotTrajectory, JointTrajectory, RobotState or JointState, got {}".format(str(type(trajectory))))
self._display_traj.publish(dt)
def compute_cartesian_path(
self,
waypoints,
eef_step,
jump_threshold,
avoid_collisions=True,
path_constraints=None,
):
""" Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """
if path_constraints:
if type(path_constraints) is Constraints:
constraints_str = conversions.msg_to_string(path_constraints)
else:
raise MoveItCommanderException(
"Unable to set path constraints, unknown constraint type "
+ type(path_constraints))
(ser_path, fraction) = self._g.compute_cartesian_path(
[conversions.pose_to_list(p) for p in waypoints],
eef_step,
jump_threshold,
avoid_collisions,
constraints_str,
)
else:
(ser_path, fraction) = self._g.compute_cartesian_path(
[conversions.pose_to_list(p) for p in waypoints],
eef_step,
jump_threshold,
avoid_collisions,
)
path = RobotTrajectory()
path.deserialize(ser_path)
return (path, fraction)
def retime_trajectory(self, ref_state_in, traj_in, velocity_scaling_factor):
ser_ref_state_in = conversions.msg_to_string(ref_state_in)
ser_traj_in = conversions.msg_to_string(traj_in)
ser_traj_out = self._g.retime_trajectory(ser_ref_state_in, ser_traj_in, velocity_scaling_factor)
traj_out = RobotTrajectory()
traj_out.deserialize(ser_traj_out)
return traj_out
def plan(self, joints=None):
""" Return a tuple of the motion planning results such as
(success flag : boolean, trajectory message : RobotTrajectory,
planning time : float, error code : MoveitErrorCodes) """
if type(joints) is JointState:
self.set_joint_value_target(joints)
elif type(joints) is Pose:
self.set_pose_target(joints)
elif joints is not None:
try:
self.set_joint_value_target(self.get_remembered_joint_values()[joints])
except MoveItCommanderException:
self.set_joint_value_target(joints)
(error_code_msg, trajectory_msg, planning_time) = self._g.plan()
error_code = MoveItErrorCodes()
error_code.deserialize(error_code_msg)
plan = RobotTrajectory()
return (error_code.val == MoveItErrorCodes.SUCCESS,
plan.deserialize(trajectory_msg),
planning_time,
error_code)
def runTrajectory(req):
global Traj_server;
print "---------------------------------"
print req.task
print " "
print req.bin_num
print " "
print req.using_torso
print "---------------------------------"
if req.using_torso == "y":
file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/Torso/bin"+req.bin_num);
else:
file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/bin"+req.bin_num);
if req.task == "Forward":
file_name = file_root+"/forward";
elif req.task == "Drop":
file_name = file_root+"/drop";
else :
return taskResponse(False);
f = open(file_name,"r");
plan_file = RobotTrajectory();
buf = f.read();
plan_file.deserialize(buf);
plan = copy.deepcopy(plan_file);
arm_right_group = moveit_commander.MoveGroupCommander("arm_right");
arm_left_group = moveit_commander.MoveGroupCommander("arm_left");
arm_left_group.set_start_state_to_current_state();
StartPnt = JointTrajectoryPoint();
StartPnt.positions = arm_left_group.get_current_joint_values();
StartPnt.velocities = [0]*len(StartPnt.positions);
StartPnt. accelerations = [0]*len(StartPnt.positions);
#print StartPnt;
plan.joint_trajectory.points[0] = StartPnt;
#print plan;
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory)
display_trajectory = moveit_msgs.msg.DisplayTrajectory();
robot = moveit_commander.RobotCommander();
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory);
print "============ Waiting while", file_name, " is visualized (again)..."
arm_left_group.execute(plan);
print "Trajectory ", file_name, " finished!"
f.close();
return taskResponse(True);
def create_tracking_trajectory(self, traj, speed, min_speed):
# Create a new trajectory object
new_traj = RobotTrajectory()
# Initialize the new trajectory to be the same as the input trajectory
new_traj.joint_trajectory = deepcopy(traj.joint_trajectory)
# Get the number of joints involved
n_joints = len(traj.joint_trajectory.joint_names)
# Get the number of points on the trajectory
n_points = len(traj.joint_trajectory.points)
# Cycle through all points and joints and scale the time from start,
# as well as joint speed and acceleration
for i in range(n_points):
# The joint positions are not scaled so pull them out first
new_traj.joint_trajectory.points[
i].positions = traj.joint_trajectory.points[i].positions
# Next, scale the time_from_start for this point
new_traj.joint_trajectory.points[
i].time_from_start = traj.joint_trajectory.points[
i].time_from_start / speed
# Initialize the joint velocities for this point
new_traj.joint_trajectory.points[
i].velocities = [speed] * n_joints + [min_speed] * n_joints
# Get the joint accelerations for this point
new_traj.joint_trajectory.points[i].accelerations = [speed / 4.0
] * n_joints
# Return the new trajecotry
return new_traj
def scale_trajectory_speed(traj, scale):
new_traj = RobotTrajectory()
# Initialize the new trajectory to be the same as the input trajectory
new_traj.joint_trajectory = traj.joint_trajectory
# Get the number of joints involved
n_joints = len(traj.joint_trajectory.joint_names)
# Get the number of points on the trajectory
n_points = len(traj.joint_trajectory.points)
# Store the trajectory points
points = list(traj.joint_trajectory.points)
# Cycle through all points and joints and scale the time from start,
# as well as joint speed and acceleration
for i in range(n_points):
point = JointTrajectoryPoint()
# The joint positions are not scaled so pull them out first
point.positions = traj.joint_trajectory.points[i].positions
# Next, scale the time_from_start for this point
point.time_from_start = traj.joint_trajectory.points[
i].time_from_start / scale
# Get the joint velocities for this point
point.velocities = list(traj.joint_trajectory.points[i].velocities)
# Get the joint accelerations for this point
point.accelerations = list(
traj.joint_trajectory.points[i].accelerations)
# Scale the velocity and acceleration for each joint at this point
for j in range(n_joints):
point.velocities[j] = point.velocities[j] * scale
point.accelerations[j] = point.accelerations[j] * scale * scale
# Store the scaled trajectory point
points[i] = point
# Assign the modified points to the new trajectory
new_traj.joint_trajectory.points = points
# Return the new trajecotry
return new_traj
def to_robot_trajectory(self, num_waypoints=300, jointspace=True):
"""
Parameters
----------
num_waypoints : float
how many points in the :obj:`moveit_msgs.msg.RobotTrajectory`
jointspace : bool
What kind of trajectory. Joint space points are 7x' and describe the
angle of each arm. Workspace points are 3x', and describe the x,y,z
position of the end effector.
"""
traj = JointTrajectory()
traj.joint_names = self.limb.joint_names()
points = []
for t in np.linspace(0, self.total_time, num=num_waypoints):
point = self.trajectory_point(t, jointspace)
points.append(point)
# We want to make a final point at the end of the trajectory so that the
# controller has time to converge to the final point.
extra_point = self.trajectory_point(self.total_time, jointspace)
extra_point.time_from_start = rospy.Duration.from_sec(self.total_time + 1)
points.append(extra_point)
traj.points = points
traj.header.frame_id = 'base'
robot_traj = RobotTrajectory()
robot_traj.joint_trajectory = traj
return robot_traj
def scale_trajectory_speed(traj, scale):
new_traj = RobotTrajectory()
new_traj.joint_trajectory = traj.joint_trajectory
n_joints = len(traj.joint_trajectory.joint_names)
n_points = len(traj.joint_trajectory.points)
points = list(traj.joint_trajectory.points)
for i in range(n_points):
point = JointTrajectoryPoint()
point.positions = traj.joint_trajectory.points[i].positions
point.time_from_start = traj.joint_trajectory.points[
i].time_from_start / scale
point.velocities = list(traj.joint_trajectory.points[i].velocities)
point.accelerations = list(
traj.joint_trajectory.points[i].accelerations)
for j in range(n_joints):
point.velocities[j] = point.velocities[j] * scale
point.accelerations[j] = point.accelerations[j] * scale * scale
points[i] = point
new_traj.joint_trajectory.points = points
return new_traj
def robotTrajectoryFromPlanPoseBasedDownSampledAndWithDMP(
self, plan, groups):
"""Given a dmp plan (GetDMPPlanResponse) create a RobotTrajectory doing IK calls for the PoseStampeds
provided so we can visualize and execute the motion"""
rt = RobotTrajectory()
jt = JointTrajectory()
#jt.joint_names
jt.points
# Magic code goes here.
fjtg = self.computeJointTrajFromCartesian(plan.plan.points,
plan.plan.times)
rt.joint_trajectory = fjtg.trajectory
#plan.plan.times should be the times used
if len(rt.joint_trajectory.points) != len(plan.plan.times):
rospy.logerr(
"joint trajectory point does not have same length than planned times, this means there are IKs that failed"
)
rospy.logerr("points: " + str(len(rt.joint_trajectory.points)) +
" times: " + str(len(plan.plan.times)))
# for point, time in zip(rt.joint_trajectory.points, plan.plan.times):
# # Probably need to allocate it again here...
# point.time_from_start = rospy.Duration(time)
return rt
def scale_trajectory_speed(traj, scale):
# Create a new trajectory object
new_traj = RobotTrajectory()
# Initialize the new trajectory to be the same as the planned trajectory
new_traj.joint_trajectory = traj.joint_trajectory
# Get the number of joints involved
n_joints = len(traj.joint_trajectory.joint_names)
# Get the number of points on the trajectory
n_points = len(traj.joint_trajectory.points)
# Store the trajectory points
points = list(traj.joint_trajectory.points)
# Cycle through all points and scale the time from start, speed and acceleration
for i in range(n_points):
point = JointTrajectoryPoint()
point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
point.velocities = list(traj.joint_trajectory.points[i].velocities)
point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
point.positions = traj.joint_trajectory.points[i].positions
for j in range(n_joints):
point.velocities[j] = point.velocities[j] * scale
point.accelerations[j] = point.accelerations[j] * scale * scale
points[i] = point
# Assign the modified points to the new trajectory
new_traj.joint_trajectory.points = points
# Return the new trajecotry
return new_traj
def visualize_movement(start, path):
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', DisplayTrajectory,
queue_size=100) # for visualizing the robot movement;
sleep(0.5)
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = convertStateToRobotState(start)
trajectory = RobotTrajectory()
points = []
joint_trajectory = JointTrajectory()
joint_trajectory.joint_names = get_joint_names('right')
for state, _ in path:
point = JointTrajectoryPoint()
point.positions = convertStateToRobotState(state).joint_state.position
points.append(point)
joint_trajectory.points = points
trajectory.joint_trajectory = joint_trajectory
# print("The joint trajectory is: %s" % trajectory)
display_trajectory.trajectory.append(trajectory)
display_trajectory_publisher.publish(display_trajectory)
def retime_trajectory(self, ref_state_in, traj_in, velocity_scaling_factor):
ser_ref_state_in = conversions.msg_to_string(ref_state_in)
ser_traj_in = conversions.msg_to_string(traj_in)
ser_traj_out = self._g.retime_trajectory(ser_ref_state_in, ser_traj_in, velocity_scaling_factor)
traj_out = RobotTrajectory()
traj_out.deserialize(ser_traj_out)
return traj_out
def display(self, trajectory):
"""
Display a joint-space trajectory or a robot state in RVIz loaded with the Moveit plugin
:param trajectory: a RobotTrajectory, JointTrajectory, RobotState or JointState message
"""
# Publish the DisplayTrajectory (only for trajectory preview in RViz)
# includes a convert of float durations in rospy.Duration()
def js_to_rt(js):
rt = RobotTrajectory()
rt.joint_trajectory.joint_names = js.name
rt.joint_trajectory.points.append(
JointTrajectoryPoint(positions=js.position))
return rt
dt = DisplayTrajectory()
if isinstance(trajectory, RobotTrajectory):
dt.trajectory.append(trajectory)
elif isinstance(trajectory, JointTrajectory):
rt = RobotTrajectory()
rt.joint_trajectory = trajectory
dt.trajectory.append(rt)
elif isinstance(trajectory, RobotState):
dt.trajectory.append(js_to_rt(trajectory.joint_state))
elif isinstance(trajectory, JointState):
dt.trajectory.append(js_to_rt(trajectory))
else:
raise NotImplementedError(
"ArmCommander.display() expected type RobotTrajectory, JointTrajectory, RobotState or JointState, got {}"
.format(str(type(trajectory))))
self._display_traj.publish(dt)
def interpolate_joint_space(self, goal, total_time, nb_points, start=None):
"""
Interpolate a trajectory from a start state (or current state) to a goal in joint space
:param goal: A RobotState to be used as the goal of the trajectory
param total_time: The time to execute the trajectory
:param nb_points: Number of joint-space points in the final trajectory
:param start: A RobotState to be used as the start state, joint order must be the same as the goal
:return: The corresponding RobotTrajectory
"""
dt = total_time * (1.0 / nb_points)
# create the joint trajectory message
traj_msg = JointTrajectory()
rt = RobotTrajectory()
if start == None:
start = self.get_current_state()
joints = []
start_state = start.joint_state.position
goal_state = goal.joint_state.position
for j in range(len(goal_state)):
pose_lin = np.linspace(start_state[j], goal_state[j],
nb_points + 1)
joints.append(pose_lin[1:].tolist())
for i in range(nb_points):
point = JointTrajectoryPoint()
for j in range(len(goal_state)):
point.positions.append(joints[j][i])
# append the time from start of the position
point.time_from_start = rospy.Duration.from_sec((i + 1) * dt)
# append the position to the message
traj_msg.points.append(point)
# put name of joints to be moved
traj_msg.joint_names = self.joint_names()
# send the message
rt.joint_trajectory = traj_msg
return rt
def retime_trajectory(self, ref_state_in, traj_in, velocity_scaling_factor=1.0, acceleration_scaling_factor=1.0, algorithm="iterative_time_parameterization"):
ser_ref_state_in = conversions.msg_to_string(ref_state_in)
ser_traj_in = conversions.msg_to_string(traj_in)
ser_traj_out = self._g.retime_trajectory(ser_ref_state_in, ser_traj_in, velocity_scaling_factor, acceleration_scaling_factor, algorithm)
traj_out = RobotTrajectory()
traj_out.deserialize(ser_traj_out)
return traj_out
def plan(self, joints=None):
"""Return a tuple of the motion planning results such as
(success flag : boolean, trajectory message : RobotTrajectory,
planning time : float, error code : MoveitErrorCodes)"""
if type(joints) is JointState:
self.set_joint_value_target(joints)
elif type(joints) is Pose:
self.set_pose_target(joints)
elif joints is not None:
try:
self.set_joint_value_target(
self.get_remembered_joint_values()[joints])
except MoveItCommanderException:
self.set_joint_value_target(joints)
(error_code_msg, trajectory_msg, planning_time) = self._g.plan()
error_code = MoveItErrorCodes()
error_code.deserialize(error_code_msg)
plan = RobotTrajectory()
return (
error_code.val == MoveItErrorCodes.SUCCESS,
plan.deserialize(trajectory_msg),
planning_time,
error_code,
)
def run_joint_trajectory(self, joint_trajectory):
"""
Moves robot through all joint states with specified timeouts
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
"""
plan = RobotTrajectory()
plan.joint_trajectory = joint_trajectory
self._move_group_commander.execute(plan)
def get_display_trajectory(self, *joint_trajectories):
display_trajectory = DisplayTrajectory()
display_trajectory.model_id = 'pr2'
for joint_trajectory in joint_trajectories:
robot_trajectory = RobotTrajectory()
robot_trajectory.joint_trajectory = joint_trajectory
# robot_trajectory.multi_dof_joint_trajectory = ...
display_trajectory.trajectory.append(robot_trajectory)
display_trajectory.trajectory_start = self.get_robot_state()
return display_trajectory
def compute_cartesian_path(self,
waypoints,
eef_step,
jump_threshold,
avoid_collisions=True):
""" Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """
(ser_path, fraction) = self._g.compute_cartesian_path(
[conversions.pose_to_list(p) for p in waypoints], eef_step,
jump_threshold, avoid_collisions)
path = RobotTrajectory()
path.deserialize(ser_path)
return (path, fraction)
def reset_module(self):
"""
Reset all the module's parameters to their start values.
"""
self.template = ""
self.stamps = None
self.obj_traj = None
self.left_offset = []
self.left_poses = None
self.left_traj = RobotTrajectory()
self.right_offset = []
self.right_poses = None
self.right_traj = RobotTrajectory()
def cascade_plans(plan1, plan2):
'''
Joins two robot motion plans into one
inputs: two robot trajactories
outputs: final robot trjactory
'''
# Create a new trajectory object
new_traj = RobotTrajectory()
# Initialize the new trajectory to be the same as the planned trajectory
traj_msg = JointTrajectory()
# Get the number of joints involved
n_joints1 = len(plan1.joint_trajectory.joint_names)
n_joints2 = len(plan2.joint_trajectory.joint_names)
# Get the number of points on the trajectory
n_points1 = len(plan1.joint_trajectory.points)
n_points2 = len(plan2.joint_trajectory.points)
# Store the trajectory points
points1 = list(plan1.joint_trajectory.points)
points2 = list(plan2.joint_trajectory.points)
end_time = plan1.joint_trajectory.points[n_points1 - 1].time_from_start
start_time = plan1.joint_trajectory.points[0].time_from_start
duration = end_time - start_time
# add a time toleracne between successive plans
time_tolerance = rospy.Duration.from_sec(0.1)
for i in range(n_points1):
point = JointTrajectoryPoint()
point.time_from_start = plan1.joint_trajectory.points[
i].time_from_start
point.velocities = list(plan1.joint_trajectory.points[i].velocities)
point.accelerations = list(
plan1.joint_trajectory.points[i].accelerations)
point.positions = plan1.joint_trajectory.points[i].positions
points1[i] = point
traj_msg.points.append(point)
end_time = plan1.joint_trajectory.points[i].time_from_start
for i in range(n_points2):
point = JointTrajectoryPoint()
point.time_from_start = plan2.joint_trajectory.points[i].time_from_start + \
end_time + time_tolerance
point.velocities = list(plan2.joint_trajectory.points[i].velocities)
point.accelerations = list(
plan2.joint_trajectory.points[i].accelerations)
point.positions = plan2.joint_trajectory.points[i].positions
traj_msg.points.append(point)
traj_msg.joint_names = plan1.joint_trajectory.joint_names
traj_msg.header.frame_id = plan1.joint_trajectory.header.frame_id
new_traj.joint_trajectory = traj_msg
return new_traj
def slow_down(traj):
"""Slows down an exiting move it carestian path trajectory
Slows down a moveit trajectory by a factor defined in ``spd``. Iterates through all points in trajectory and scales the following
variables: ``time_from_start``, ``accelerations``, ``positions``
Args:
traj (RobotTrajectory): Standard ROS message type from moveit_msgs.msg
Returns:
RobotTrajectory: identical but slow downed RobotTrajectory object
"""
new_traj = RobotTrajectory()
new_traj.joint_trajectory = traj.joint_trajectory
n_joints = len(traj.joint_trajectory.joint_names)
n_points = len(traj.joint_trajectory.points)
spd = 0.2 # Lower this when using the Gazebo Robot
for i in range(n_points):
new_traj.joint_trajectory.points[
i].time_from_start = traj.joint_trajectory.points[
i].time_from_start / spd
# rospy.loginfo(type(traj.joint_trajectory.points[i]))
v = list(new_traj.joint_trajectory.points[i].velocities)
a = list(new_traj.joint_trajectory.points[i].accelerations)
p = list(new_traj.joint_trajectory.points[i].positions)
for j in range(n_joints):
# rospy.loginfo(type(new_traj.joint_trajectory.points[i].velocities))
v[j] = traj.joint_trajectory.points[i].velocities[j] * spd
a[j] = traj.joint_trajectory.points[i].accelerations[j] * spd**2
p[j] = traj.joint_trajectory.points[i].positions[j]
# new_traj.joint_trajectory.points[i].accelerations[j] = traj.joint_trajectory.points[i].accelerations[j] * spd
# new_traj.joint_trajectory.points[i].positions[j] = traj.joint_trajectory.points[i].positions[j]
v = tuple(v)
a = tuple(a)
p = tuple(p)
new_traj.joint_trajectory.points[i].velocities = v
new_traj.joint_trajectory.points[i].accelerations = a
new_traj.joint_trajectory.points[i].positions = p
# rospy.loginfo( new_traj.joint_trajectory.points[i].velocities[j])
# rospy.loginfo( new_traj.joint_trajectory.points[i].accelerations[j])
# rospy.loginfo( new_traj.joint_trajectory.points[i].positions[j])
return new_traj
def create_trajectory(self, q_list):
jt = JointTrajectory()
jt.header.frame_id = self.frame_id
jt.points = []
for i in range(0, len(q_list)):
p = JointTrajectoryPoint()
p.positions = q_list[i, :]
p.time_from_start.secs = i / 10.
p.time_from_start.nsecs = i / 10. + 0.002
jt.points.append(p)
jt.joint_names = self.joint_names
rt = RobotTrajectory()
rt.joint_trajectory = jt
return rt # (rt for move_group; jt for motion_planning)
def create_display_traj(self, joint_traj):
self.display_traj = DisplayTrajectory()
self.display_traj.model_id = 'lbr4'
robot_traj = RobotTrajectory()
robot_traj.joint_trajectory = joint_traj
self.display_traj.trajectory.append(robot_traj)
self.display_traj.trajectory_start.\
joint_state.header.frame_id = '/world'
self.display_traj.trajectory_start.joint_state.name = \
self.kdl_kin.get_joint_names()
self.display_traj.trajectory_start.joint_state.position = \
joint_traj.points[0].positions
self.display_traj.trajectory_start.joint_state.velocity = \
joint_traj.points[0].velocities
def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, avoid_collisions=True, path_constraints=None):
""" Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """
if path_constraints:
if type(path_constraints) is Constraints:
constraints_str = conversions.msg_to_string(path_constraints)
else:
raise MoveItCommanderException("Unable to set path constraints, unknown constraint type " + type(path_constraints))
(ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions, constraints_str)
else:
(ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions)
path = RobotTrajectory()
path.deserialize(ser_path)
return (path, fraction)
def viz_joint_traj(self, j_traj, base_link='base_link'):
robot_tr = RobotTrajectory()
j_traj.header.frame_id = base_link
robot_tr.joint_trajectory = j_traj
disp_tr = DisplayTrajectory()
disp_tr.trajectory.append(robot_tr)
disp_tr.model_id = self.robot_name
i = 0
while (not rospy.is_shutdown() and i < 10):
i += 1
self.pub_tr.publish(disp_tr)
self.rate.sleep()
def _convertTrajectory(self, trajectory):
points = []
for wp in trajectory.waypoints:
timeStamp = rostime.Duration(secs=wp.timestamp[0],
nsecs=wp.timestamp[1])
jtp = JointTrajectoryPoint(positions=wp.positions,
velocities=wp.velocities,
accelerations=wp.accelerations,
time_from_start=timeStamp)
points.append(jtp)
jointTraj = JointTrajectory(joint_names=trajectory.joint_names,
points=points)
robotTraj = RobotTrajectory()
robotTraj.joint_trajectory = jointTraj
return robotTraj
def runTrajectory(req):
global Traj_server
print "---------------------------------"
print req.task
print " "
print req.bin_num
print " "
print req.using_torso
print "---------------------------------"
if req.using_torso == "y":
file_root = os.path.join(os.path.dirname(__file__),
"../trajectories/Torso/bin" + req.bin_num)
else:
file_root = os.path.join(os.path.dirname(__file__),
"../trajectories/bin" + req.bin_num)
if req.task == "Forward":
file_name = file_root + "/forward"
elif req.task == "Drop":
file_name = file_root + "/drop"
elif req.task == "Pick":
file_name = file_root + "/Pick"
elif req.task == "Scan":
file_name = file_root + "/scan"
elif req.task == "Dump":
file_name = file_root + "/Dump"
elif req.task == "Lift":
file_name = file_root + "/Lift"
elif req.task == "Home":
file_name = file_root + "/Home"
elif req.task == "Rotate":
file_name = file_root + "/Rotate"
else:
return taskResponse(False)
f = open(file_name, "r")
plan_file = RobotTrajectory()
buf = f.read()
plan_file.deserialize(buf)
plan = copy.deepcopy(plan_file)
f.close()
return GetTrajectoryResponse(plan, True)
def get_box_plan(self, num, start_state, grasp):
# Set argument start state
moveit_start_state = RobotState()
moveit_start_state.joint_state = start_state
self.arm.set_start_state(moveit_start_state)
# Calculate goal pose
self.arm.set_joint_value_target(self.box_pose[num])
# plan
plan = RobotTrajectory()
counter = 0
while len(plan.joint_trajectory.points) == 0:
plan = self.arm.plan()
counter += 1
self.arm.set_planning_time(self.planning_limitation_time +
counter * 5.0)
if counter > 1:
return (False, start_state)
self.arm.set_planning_time(self.planning_limitation_time)
rospy.loginfo("!! Got a box plan !!")
# publish the plan
pub_msg = HandringPlan()
pub_msg.grasp = grasp
pub_msg.trajectory = plan
self.hp_pub.publish(pub_msg)
self.arm.clear_pose_targets()
# return goal state from generated trajectory
goal_state = JointState()
goal_state.header = Header()
goal_state.header.stamp = rospy.Time.now()
goal_state.name = plan.joint_trajectory.joint_names[:]
goal_state.position = plan.joint_trajectory.points[-1].positions[:]
return (True, goal_state, plan)
def plan(self, joints = None):
""" Return a motion plan (a RobotTrajectory) to the set goal state (or specified by the joints argument) """
if type(joints) is JointState:
self.set_joint_value_target(joints)
elif type(joints) is Pose:
self.set_pose_target(joints)
elif not joints == None:
try:
self.set_joint_value_target(self.get_remembered_joint_values()[joints])
except:
self.set_joint_value_target(joints)
plan = RobotTrajectory()
plan.deserialize(self._g.compute_plan())
return plan
def plan(self, joints = None):
""" Return a motion plan (a RobotTrajectory) to the set goal state (or specified by the joints argument) """
if type(joints) is JointState:
self.set_joint_value_target(joints)
elif type(joints) is Pose:
self.set_pose_target(joints)
elif not joints == None:
try:
self.set_joint_value_target(self.get_remembered_joint_values()[joints])
except:
self.set_joint_value_target(joints)
plan = RobotTrajectory()
plan.deserialize(self._g.compute_plan())
return plan
def plan_point(goal):
"""
Function that generates the plans for the point action
"""
plans = []
hand_id = -1
if goal.position.y > 0.0:
hand_id = yumi.LEFT
move_group = yumi.group_l
else:
hand_id = yumi.RIGHT
move_group = yumi.group_r
if goal.position.z < 0.3:
goal.position.z = 0.3
pose_ee_t = [
goal.position.x, goal.position.y, goal.position.z, 0.0, 3.14, 0.0
]
plan = RobotTrajectory()
plan = yumi.plan_path_local([pose_ee_t], hand_id)
if (plan and len(plan.joint_trajectory.points) != 0):
plans.append(plan)
else:
return []
return plans
def handle_execute_request(self, req):
if "manipulator" in req.REQUEST_TYPE:
self.group = moveit_commander.MoveGroupCommander("manipulator")
elif "gripper" in req.REQUEST_TYPE:
self.group = moveit_commander.MoveGroupCommander("gripper")
else:
rospy.logwarn("PlanningService::handle_plan_request() -- requested group is not recognized")
return False
#build moveit msg for display
joint_multi_traj_msg = MultiDOFJointTrajectory()
robot_traj_msg = RobotTrajectory()
robot_traj_msg.joint_trajectory = req.JOINT_TRAJECTORY
robot_traj_msg.multi_dof_joint_trajectory = joint_multi_traj_msg
self.group.execute(robot_traj_msg, wait=True)
def handle_show_request(self, req):
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self.robot.get_current_state()
#build moveit msg for display
joint_multi_traj_msg = MultiDOFJointTrajectory()
robot_traj_msg = RobotTrajectory()
robot_traj_msg.joint_trajectory = req.JOINT_TRAJECTORY
robot_traj_msg.multi_dof_joint_trajectory = joint_multi_traj_msg
display_trajectory.trajectory.append(robot_traj_msg)
rospy.loginfo("PlanningService::handle_show_request() -- showing trajectory %s", req.JOINT_TRAJECTORY)
self.display_trajectory_publisher.publish(display_trajectory);
return True
def scale_trajectory_speed(traj, scale):
# Create a new trajectory object
new_traj = RobotTrajectory()
# Initialize the new trajectory to be the same as the input trajectory
new_traj.joint_trajectory = traj.joint_trajectory
# Get the number of joints involved
n_joints = len(traj.joint_trajectory.joint_names)
# Get the number of points on the trajectory
n_points = len(traj.joint_trajectory.points)
# Store the trajectory points
points = list(traj.joint_trajectory.points)
# Cycle through all points and joints and scale the time from start,
# as well as joint speed and acceleration
for i in range(n_points):
point = JointTrajectoryPoint()
# The joint positions are not scaled so pull them out first
point.positions = traj.joint_trajectory.points[i].positions
# Next, scale the time_from_start for this point
point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
# Get the joint velocities for this point
point.velocities = list(traj.joint_trajectory.points[i].velocities)
# Get the joint accelerations for this point
point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
# Scale the velocity and acceleration for each joint at this point
for j in range(n_joints):
point.velocities[j] = point.velocities[j] * scale
point.accelerations[j] = point.accelerations[j] * scale * scale
# Store the scaled trajectory point
points[i] = point
# Assign the modified points to the new trajectory
new_traj.joint_trajectory.points = points
# Return the new trajecotry
return new_traj
def visualize_trajectory(self, blocking=True):
if not self.display:
return
if not self.single_arm:
self.adjust_traj_length()
if self.rviz_pub.get_num_connections() < 1:
rospy.logerr("Rviz topic not subscribed")
return
msg = DisplayTrajectory()
msg.trajectory_start = self.robot_model.get_current_state()
traj = RobotTrajectory()
goal = JointTrajectory()
goal.joint_names = self.left.JOINT_NAMES[:]
if not self.single_arm:
goal.joint_names += self.right.JOINT_NAMES[:]
# The sim is very slow if the number of points is too large
steps = 1 if len(self.left._goal.trajectory.points) < 100 else 10
for idx in range(0, len(self.left._goal.trajectory.points), steps):
comb_point = JointTrajectoryPoint()
lpt = self.left._goal.trajectory.points[idx]
comb_point.positions = lpt.positions[:]
if not self.single_arm:
comb_point.positions += self.right._goal.trajectory.points[idx].positions[:]
comb_point.time_from_start = lpt.time_from_start
goal.points.append(comb_point)
traj.joint_trajectory = goal
msg.trajectory.append(traj)
duration = goal.points[-1].time_from_start.to_sec()
self.rviz_pub.publish(msg)
if blocking:
rospy.loginfo("Waiting for trajectory animation {} seconds".format(duration))
rospy.sleep(duration)
def robotTrajectoryFromPlanPoseBased(self, plan, groups, downsample_freq=None):
"""Given a dmp plan (GetDMPPlanResponse) create a RobotTrajectory doing IK calls for the PoseStampeds
provided so we can visualize and execute the motion"""
rt = RobotTrajectory()
jt = JointTrajectory()
#jt.joint_names
jt.points
# Magic code goes here.
fjtg = self.computeJointTrajFromCartesian(plan.plan.points, plan.plan.times, downsample_freq=downsample_freq)
rt.joint_trajectory = fjtg.trajectory
#plan.plan.times should be the times used
if len(rt.joint_trajectory.points) != len(plan.plan.times):
rospy.logerr("joint trajectory point does not have same length than planned times, this means there are IKs that failed")
rospy.logerr("points: " + str(len(rt.joint_trajectory.points)) + " times: " + str(len(plan.plan.times)))
# for point, time in zip(rt.joint_trajectory.points, plan.plan.times):
# # Probably need to allocate it again here...
# point.time_from_start = rospy.Duration(time)
return rt
def plan(self, joints=None):
""" Return a motion plan (a RobotTrajectory) to the set goal state (or specified by the joints argument) """
if type(joints) is JointState:
self._g.set_joint_value_target(joints.position)
elif type(joints) is Pose:
self.set_pose_target(joints)
elif not joints == None:
try:
self.set_joint_value_target(self.get_remembered_joint_values()[joints])
except:
self.set_joint_value_target(joints)
plan = self._g.get_plan()
plan_msg = RobotTrajectory()
joint_traj = JointTrajectory()
joint_traj.joint_names = plan["joint_trajectory"]["joint_names"]
for point in plan["joint_trajectory"]["points"]:
joint_traj.points.append(
JointTrajectoryPoint(
positions=point["positions"], velocities=point["velocities"], accelerations=point["accelerations"]
)
)
multi_dof_joint_traj = MultiDOFJointTrajectory()
multi_dof_joint_traj.joint_names = plan["multi_dof_joint_trajectory"]["joint_names"]
multi_dof_joint_traj.frame_ids = plan["multi_dof_joint_trajectory"]["frame_ids"]
multi_dof_joint_traj.child_frame_ids = plan["multi_dof_joint_trajectory"]["child_frame_ids"]
for point in plan["multi_dof_joint_trajectory"]["points"]:
multi_dof_joint_traj_point = MultiDOFJointTrajectoryPoint()
for t in point["transforms"]:
multi_dof_joint_traj_point.poses.append(
Transform(
translation=Vector(x=t["translation"]["x"], y=t["translation"]["y"], z=t["translation"]["z"]),
rotation=Quaternion(
x=t["rotation"]["x"], y=t["rotation"]["y"], z=t["rotation"]["z"], w=t["rotation"]["w"]
),
)
)
multi_dof_joint_traj.points.append(multi_dof_joint_traj_point)
plan_msg.joint_trajectory = joint_traj
plan_msg.multi_dof_joint_trajectory = multi_dof_joint_traj
return plan_msg
def createRobotTrajectoryFromJointStates(joint_states, interesting_joint_list):
"""Given a joint_states message and the joints we are interested in moving create
a RobotTrajectory that satisfies it"""
curr_joint_states = copy.deepcopy(joint_states)
rt = RobotTrajectory()
jt = JointTrajectory()
jt_point = JointTrajectoryPoint()
jt.joint_names = interesting_joint_list
jt.header.stamp = rospy.Time.now()
for joint in interesting_joint_list:
# Search for the position of the joint in the joint_states message
idx_joint = curr_joint_states.name.index(joint)
jt_point.positions.append(curr_joint_states.position[idx_joint])
jt_point.velocities.append(0.0) # Set speeds to 0.0, in theory movements will be little and also with no effort
jt_point.time_from_start = rospy.Duration(0.3) # Arbitrary time, we are not dealing with speed issues here
jt.points.append(jt_point)
rt.joint_trajectory = jt
return rt
def dict_to_trajectory(plan):
plan_msg = RobotTrajectory()
joint_traj = JointTrajectory()
joint_traj.header.frame_id = plan["joint_trajectory"]["frame_id"]
joint_traj.joint_names = plan["joint_trajectory"]["joint_names"]
for point in plan["joint_trajectory"]["points"]:
joint_traj.points.append(JointTrajectoryPoint(
positions = point["positions"],
velocities = point["velocities"],
accelerations = point["accelerations"],
time_from_start = rospy.Duration().from_sec(point["time_from_start"])))
multi_dof_joint_traj = MultiDOFJointTrajectory()
multi_dof_joint_traj.header.frame_id = plan["multi_dof_joint_trajectory"]["frame_id"]
multi_dof_joint_traj.joint_names = plan["multi_dof_joint_trajectory"]["joint_names"]
for point in plan["multi_dof_joint_trajectory"]["points"]:
multi_dof_joint_traj_point = MultiDOFJointTrajectoryPoint()
for t in point["transforms"]:
multi_dof_joint_traj_point.transforms.append(list_to_transform(t))
multi_dof_joint_traj_point.time_from_start = rospy.Duration().from_sec(point["time_from_start"])
multi_dof_joint_traj.points.append(multi_dof_joint_traj_point)
plan_msg.joint_trajectory = joint_traj
plan_msg.multi_dof_joint_trajectory = multi_dof_joint_traj
return plan_msg
def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, avoid_collisions = True):
""" Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """
(ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions)
path = RobotTrajectory()
path.deserialize(ser_path)
return (path, fraction)
