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 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 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 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 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 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 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 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 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 set_trajectory_speed(traj, 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 = 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
time_list = []
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
t = rospy.Time(point.time_from_start.secs, point.time_from_start.nsecs)
# ipdb.set_trace()
time_list.append(t.to_time())
# Initialize the joint velocities for this point
point.velocities = [speed] * n_joints
# Get the joint accelerations for this point
point.accelerations = [speed / 4.0] * n_joints
# Store the scaled trajectory point
points[i] = point
# Assign the modified points to the new trajectory
new_traj.joint_trajectory.points = points
# ipdb.set_trace()
# plt.plot(time_list)
# plt.show()
# Return the new trajecotry
return new_traj
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 convertPlanToTrajectory(path):
"""
Convert a path in OMPL to a trajectory in Moveit that could be used for display;
:param plan: A path in OMPL;
:return: a RobotTrajectory message in Moveit;
"""
trajectory = RobotTrajectory()
states = path.getStates()
points = []
joint_trajectory = JointTrajectory()
joint_trajectory.joint_names = get_joint_names('right')
for state in states:
point = JointTrajectoryPoint()
point.positions = convertStateToRobotState(state).joint_state.position
points.append(point)
joint_trajectory.points = points
trajectory.joint_trajectory = joint_trajectory
return trajectory
def reverse_planning(self, plan):
rev_plan = RobotTrajectory()
rev_joint_traj = JointTrajectory()
rev_joint_traj.header = copy.deepcopy(plan.joint_trajectory.header)
rev_joint_traj.joint_names = copy.deepcopy(plan.joint_trajectory.joint_names)
new_points = []
points = plan.joint_trajectory.points # JointTrajectoryPoint[]
l = len(points)
'''
How to reverse the cartesian plan?
Reverse the order of trajectory point's joint values.
Change the sign of velocity and acceleration.
Change the accumulative time parameterization to follow
the reverse order time gap.
But you should put the current joint value to the first
trajectory point. If not, the planner will return error due to
joint deviation.
'''
for i in range(l):
point = JointTrajectoryPoint()
if i == 0:
point.positions = self.ur5.get_current_joint_values()
point.velocities = [0.0 for val in point.positions]
point.accelerations = [0.0 for val in point.positions]
point.time_from_start = rospy.Duration(0)
else:
point.positions = copy.deepcopy(points[l-1-i].positions)
point.velocities = [-value for value in points[l-1-i].velocities]
point.accelerations = [-value for value in points[l-1-i].accelerations]
point.time_from_start = new_points[i-1].time_from_start + points[l-i].time_from_start - points[l-1-i].time_from_start
new_points.append(point)
rev_joint_traj.points = new_points
rev_plan.joint_trajectory = rev_joint_traj
return rev_plan
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 create_trajectory(self, q_list, v_list, a_list, t):
tnsec, tsec = np.modf(t)
jt = JointTrajectory()
jt.header.frame_id = '/panda_link0'
jt.points = []
for i, tm in enumerate(t):
p = JointTrajectoryPoint()
p.positions = q_list[i, :]
p.velocities = v_list[i, :]
p.accelerations = a_list[i, :]
p.time_from_start.secs = tsec[i]
p.time_from_start.nsecs = tnsec[i] * 1e09
jt.points.append(p)
jt.joint_names = [
"panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4",
"panda_joint5", "panda_joint6", "panda_joint7"
]
rt = RobotTrajectory()
rt.joint_trajectory = jt
return rt # (rt for move_group; jt for motion_planning)
def send_traj(self, u_arr):
# Formulate joint trajectory message:
jt_msg = JointTrajectory()
jt_msg.joint_names = self.joint_names
for i in range(len(u_arr)):
u = u_arr[i]
jt_pt = JointTrajectoryPoint()
jt_pt.positions = u
jt_msg.points.append(jt_pt)
robot_tr = RobotTrajectory()
robot_tr.joint_trajectory = jt_msg
disp_tr = DisplayTrajectory()
disp_tr.trajectory.append(robot_tr)
disp_tr.model_id = self.robot_name
self.pub_tr.publish(disp_tr)
i = 0
while (not rospy.is_shutdown() and i < 10):
i += 1
self.rate.sleep()
def single_shot(path, obstacles):
exp = DiffCoplusBaxterExperiments()
print('Adding box')
rospy.sleep(2)
box_names = []
## Commented out because assuming obstacles already exist in scene
# for i, obs in enumerate(obstacles):
# box_name = 'box_{}'.format(i)
# box_names.append(box_name)
# box_pose = geometry_msgs.msg.PoseStamped()
# box_pose.header.frame_id = exp.planning_frame
# # box_pose.pose.orientation.w = 1.0
# box_pose.pose.position.x = obs[1][0]
# box_pose.pose.position.y = obs[1][1]
# box_pose.pose.position.z = obs[1][2]
# exp.scene.add_box(box_name, box_pose, size=obs[2])
# wait_for_state_update(exp.scene, box_name, box_is_known=True)
pub = exp.display_trajectory_publisher
joint_traj = JointTrajectory()
for q in path:
traj_point = JointTrajectoryPoint()
traj_point.positions = q.numpy().tolist()
joint_traj.points.append(traj_point)
joint_traj.joint_names = [
'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1',
'left_w2'
]
robot_traj = RobotTrajectory()
robot_traj.joint_trajectory = joint_traj
disp_traj = DisplayTrajectory()
disp_traj.trajectory.append(robot_traj)
disp_traj.trajectory_start.joint_state.position = path[0].numpy()
disp_traj.trajectory_start.joint_state.name = joint_traj.joint_names
pub.publish(disp_traj)
return box_names, exp
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 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 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 create_tracking_trajectory_1(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 + 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 display_trajectory(self, plan):
print(plan)
traj = plan.trajectory
joints = traj.joint_names
print(traj)
init_joints = JointState()
init_joints.name = joints
init_joints.position = traj.points[0].positions
init_joints.velocity = traj.points[0].velocities
init_joints.effort = traj.points[0].effort
joint_traj = RobotTrajectory()
joint_traj.joint_trajectory = plan.trajectory
init_state = RobotState()
init_state.joint_state = init_joints
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = init_state
display_trajectory.trajectory.append(joint_traj)
# Publish
self.display_trajectory_publisher.publish(display_trajectory)
rospy.sleep(3)
def execute_plan_close_loop_with_moveit(arm_cmd_pub, gripper_cmd_pub, args):
from moveit_msgs.msg import RobotTrajectory
# ** execution of plan **
# open gripper before path
gripper_openning(gripper_cmd_pub)
rospy.loginfo("openning gripper...")
arm_cmd = args['pre_grasp_trajectory']
# construct plan
arm_traj = RobotTrajectory()
arm_traj.joint_trajectory = arm_cmd
#arm_cmd = add_velocity(arm_cmd)
group.execute(arm_traj, wait=True)
#arm_cmd_pub.publish(arm_cmd)
rospy.loginfo("pre_grasp executing...")
#arm_block_until_near(joint_trajectory=arm_cmd, pos_dif_threshold=0.01, ori_dif_threshold=10)
#arm_block_until_near(end_pose=args['pre_grasp_end_pose'])
arm_cmd = args['pre_to_grasp_trajectory']
# construct plan
arm_traj = RobotTrajectory()
arm_traj.joint_trajectory = arm_cmd
#arm_cmd = add_velocity(arm_cmd)
group.execute(arm_traj, wait=True)
#arm_cmd_pub.publish(arm_cmd)
rospy.loginfo("pre_grasp to grasp executing...")
#arm_block_until_near(joint_trajectory=arm_cmd, pos_dif_threshold=0.001, ori_dif_threshold=10)
#arm_block_until_near(end_pose=args['grasp_end_pose'])
rospy.loginfo("closing gripper...")
gripper_closing(gripper_cmd_pub)
#gripper_closing_and_store_pose(model_name, global_grasp_pose, grasp_id)
# post_grasp
arm_cmd = args['post_grasp_trajectory']
#arm_cmd = add_velocity(arm_cmd)
# construct plan
arm_traj = RobotTrajectory()
arm_traj.joint_trajectory = arm_cmd
#arm_cmd = add_velocity(arm_cmd)
group.execute(arm_traj, wait=True)
#arm_cmd_pub.publish(arm_cmd)
rospy.loginfo("post_grasp executing...")
#arm_block_until_near(joint_trajectory=arm_cmd, pos_dif_threshold=0.01, ori_dif_threshold=10)
#arm_block_until_near(end_pose=args['post_grasp_end_pose'])
# execute plan for placing
arm_cmd = args['place_trajectory']
#arm_cmd = add_velocity(arm_cmd)
# construct plan
arm_traj = RobotTrajectory()
arm_traj.joint_trajectory = arm_cmd
#arm_cmd = add_velocity(arm_cmd)
group.execute(arm_traj, wait=True)
#arm_cmd_pub.publish(arm_cmd)
rospy.loginfo("place plan executing...")
#arm_block_until_near(joint_trajectory=arm_cmd, pos_dif_threshold=0.01, ori_dif_threshold=10)
#arm_block_until_near(end_pose=args['place_end_pose'])
# detect if the object didn't slip, and the grasp is successful
store_pose_based_on_gripper(args['global_grasp_pose'], args['grasp_id'])
# detect if gripper is closed
gripper_success = detect_gripper_closing()
# open gripper
gripper_openning(gripper_cmd_pub)
rospy.sleep(0.5)
rospy.loginfo("openning gripper...")
# execute plan for resetting
# post_place_execute
arm_cmd = args['post_place_trajectory']
#arm_cmd = add_velocity(arm_cmd)
# construct plan
arm_traj = RobotTrajectory()
arm_traj.joint_trajectory = arm_cmd
#arm_cmd = add_velocity(arm_cmd)
group.execute(arm_traj, wait=True)
#arm_cmd_pub.publish(arm_cmd)
rospy.loginfo("post-place plan executing...")
#arm_block_until_near(joint_trajectory=arm_cmd, pos_dif_threshold=0.01, ori_dif_threshold=10)
#arm_block_until_near(end_pose=args['post_place_end_pose'])
arm_cmd = args['reset_trajectory']
#arm_cmd = add_velocity(arm_cmd)
# construct plan
arm_traj = RobotTrajectory()
arm_traj.joint_trajectory = arm_cmd
#arm_cmd = add_velocity(arm_cmd)
group.execute(arm_traj, wait=True)
#arm_cmd_pub.publish(arm_cmd)
rospy.loginfo("reset plan executing...")
#arm_block_until_near(joint_trajectory=arm_cmd, pos_dif_threshold=0.01, ori_dif_threshold=10)
return gripper_success
def get_robot_trajectory_msg(self, path_config):
robot_trajectory = RobotTrajectory()
robot_trajectory.joint_trajectory = self.get_joint_trajectory_msg(
path_config)
return robot_trajectory
def convert_jointspace_to_workspace_trajectory(joint_traj,
limb,
kin,
num_ways=300):
"""
PROJECT 1 PART B
Converts a jointspace RobotTrajectory into a workspace trajectory.
Recall that for workspace trajectories, we store positions as SE(3)
configurations given in the form of a 7D vector, where the first 3 entries are
the spatial x,y,z position, and the final 4 entries are the quaternion representation
of the orientation.
"""
raise NotImplementedError
# Interpolates the given joint trajectory to have num_ways waypoints. The result is an
# array of times and joint_positions (where the robot will be at those respective times).
# This is done because the trajectories returned by moveit tend to be quite sparse.
times, joint_positions = interpolate_path(joint_traj.joint_trajectory,
num_ways=num_ways)
traj = JointTrajectory()
traj.joint_names = limb.joint_names()
points = []
for i in range(len(joint_positions) - 1):
curr_pos = joint_positions[i] # current vector of joint angles.
next_pos = joint_positions[i + 1] # next vector of joint angles.
# Compute workspace configuration and body velocity.
# hint: kin.forward_position_kinematics, get_g_matrix, and g_matrix_log may be useful.
curr_workspace_config = TODO
if times[i + 1] - times[i] > 1e-5:
body_velocity = TODO
else:
body_velocity = np.zeros(6)
point = JointTrajectoryPoint()
point.positions = curr_workspace_config
point.velocities = body_velocity
point.time_from_start = rospy.Duration(times[i])
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.
last_point = JointTrajectoryPoint()
last_pos = joint_traj.joint_trajectory.points[-1].positions
last_time = joint_traj.joint_trajectory.points[-1].time_from_start
positions_dict = joint_array_to_dict(last_pos, limb)
last_workspace_config = kin.forward_position_kinematics(
joint_values=positions_dict)
last_point.positions = last_workspace_config
# What should the desired body velocity at this final position?
last_point.velocities = TODO # TODO
last_point.time_from_start = last_time
points.append(last_point)
traj.points = points
traj.header.frame_id = 'base'
robot_traj = RobotTrajectory()
robot_traj.joint_trajectory = traj
return robot_traj
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
# Connect to the right_arm move group
right_arm = moveit_commander.MoveGroupCommander('arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Display the name of the end_effector link
rospy.loginfo("The end effector link is: " + str(end_effector_link))
# Set a small tolerance on joint angles
right_arm.set_goal_tolerance(0.0001)
right_arm.set_named_target('right')
right_arm.go()
rospy.sleep(1)
'''
start_pose = right_arm.get_current_pose().pose
start_pose.position.x = 0.4
start_pose.position.y = 0
#start_pose.position.z = 0.4
start_pose.orientation.x = 0.707106781186547
start_pose.orientation.y = 0
start_pose.orientation.z = 0
start_pose.orientation.w = 0.707106781186547
right_arm.set_pose_target(start_pose)
right_arm.go()
# Pause for a moment
rospy.sleep(1)
'''
plan = RobotTrajectory()
joint_trajectory = JointTrajectory()
joint_trajectory.header.frame_id = 'base_link'
#joint_trajectory.header.stamp = rospy.Time.now()
joint_trajectory.joint_names = [
'base_to_armA', 'armA_to_armB', 'armB_to_armC', 'armC_to_armD'
]
# Output
with open("/home/jyk/Desktop/pvt.txt", 'r') as f:
for line in f.readlines():
cont = line.rstrip('\r\n').replace(' ', '').split(',')
point = JointTrajectoryPoint()
point.positions = map(float, cont[0:4])
point.velocities = map(float, cont[4:8])
point.time_from_start = rospy.Duration(float(cont[8]))
joint_trajectory.points.append(deepcopy(point))
plan.joint_trajectory = joint_trajectory
#print type(plan)
#print plan
right_arm.set_start_state_to_current_state()
right_arm.execute(plan)
#rospy.sleep(3)
'''
# Return the arm to the named "resting" pose stored in the SRDF file
right_arm.set_named_target('right')
right_arm.go()
rospy.sleep(1)
'''
# Cleanly shut down MoveIt
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
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)
# Get the current joint values
current_joint_values = self.right_arm.get_current_joint_values()
# Compute the difference from the final joint values
delta_joint_values = np.subtract(
traj.joint_trajectory.points[n_points - 1].positions,
tuple(current_joint_values))
delta_signs = list()
for i in range(n_joints):
delta_signs.append(copysign(1.0, delta_joint_values[i]))
traj_position_array = np.array(
traj.joint_trajectory.points[n_points - 1].positions)
traj_position_array += np.array(delta_signs) * 0.05
# Pull off the final joint state
#self.right_arm.set_joint_value_target(traj.joint_trajectory.points[n_points - 1].positions)
self.right_arm.set_joint_value_target(traj_position_array)
new_traj = self.right_arm.plan()
for i in range(1):
# The joint positions are not scaled so pull them out first
#new_traj.joint_trajectory.points[i].positions = traj.joint_trajectory.points[i].positions + tuple([0.1] * n_joints)
# Next, scale the time_from_start for this point
new_traj.joint_trajectory.points[
i].time_from_start = new_traj.joint_trajectory.points[
i].time_from_start / speed
# Initialize the joint velocities for this point
new_traj.joint_trajectory.points[i].velocities = [
speed + min_speed
] * n_joints
# Get the joint accelerations for this point
new_traj.joint_trajectory.points[i].accelerations = [speed / 4.0
] * n_joints
#new_traj.joint_trajectory.points = new_traj.joint_trajectory.points[n_points - 1:]
# Next, scale the time_from_start for this point
#new_traj.joint_trajectory.points[0].time_from_start = traj.joint_trajectory.points[n_points - 1].time_from_start / speed
# Initialize the joint velocities for this point
#new_traj.joint_trajectory.points[0].velocities = [speed + min_speed] * n_joints
# Get the joint accelerations for this point
#new_traj.joint_trajectory.points[0].accelerations = [speed / 4.0] * n_joints
# # 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 + tuple([0.1] * n_joints)
#
# # 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 + 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
