def displayTrajFromRobotTraj(self, robot_traj):
"""Given a robot trajectory return a displaytrajectory with it"""
dt = DisplayTrajectory()
dt.trajectory.append(robot_traj)
dt.trajectory_start.joint_state.name = robot_traj.joint_trajectory.joint_names
dt.trajectory_start.joint_state.position = robot_traj.joint_trajectory.points[0].positions
dt.model_id = "reem"
return dt
def displayTrajFromPlan(self, plan, joint_names, initial_state):
"""Given a plan (GetDMPPlanResponse), joint_names list and an initial_state as RobotState
Create a displayTraj message"""
dt = DisplayTrajectory()
dt.trajectory.append( self.robotTrajectoryFromPlan(plan, joint_names) )
dt.trajectory_start = self.robotStateFromJoints(joint_names, initial_state)
dt.model_id = "reem"
return dt
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 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 execute(self):
rospy.loginfo("Start Task")
# Get latest task plan
plan = self.task_q[0]
for points in plan.trajectory.joint_trajectory.points:
tfs = points.time_from_start.to_sec()
tfs /= self.exe_speed_rate
points.time_from_start = rospy.Duration(tfs)
scaled_vel = []
scaled_acc = []
for vel in points.velocities:
scaled_vel.append(vel * self.exe_speed_rate)
points.velocities = tuple(scaled_vel)
for acc in points.accelerations:
scaled_acc.append(acc * self.exe_speed_rate *
self.exe_speed_rate)
points.accelerations = tuple(scaled_acc)
# Display the Trajectory
start_state = JointState()
start_state.header = Header()
start_state.header.stamp = rospy.Time.now()
start_state.name = plan.trajectory.joint_trajectory.joint_names[:]
start_state.position = plan.trajectory.joint_trajectory.points[
-1].positions[:]
moveit_start_state = RobotState()
moveit_start_state.joint_state = start_state
pub_display_msg = DisplayTrajectory()
pub_display_msg.model_id = "vs087"
pub_display_msg.trajectory.append(plan.trajectory)
pub_display_msg.trajectory_start = moveit_start_state
self.display_hp_pub.publish(pub_display_msg)
# Send Action and Wait result
goal = ExecutePlanAction().action_goal.goal
goal.planning_time = plan.planning_time
goal.start_state = plan.start_state
# goal.start_state.joint_state.header.stamp = rospy.Time.now()
goal.trajectory = plan.trajectory
# rospy.logwarn(goal)
self.client.send_goal(goal)
self.client.wait_for_result()
# Update the task queue
self.task_q.pop(0)
rospy.loginfo("End Task")
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 execute(self):
# Get latest task plan
plan = self.task_q[0].trajectory
for points in plan.joint_trajectory.points:
tfs = points.time_from_start.to_sec()
tfs /= self.exe_speed_rate
points.time_from_start = rospy.Duration(tfs)
self.grasp_[0] = self.task_q[0].grasp
# Display the Trajectory
start_state = JointState()
start_state.header = Header()
start_state.header.stamp = rospy.Time.now()
start_state.name = plan.joint_trajectory.joint_names[:]
start_state.position = plan.joint_trajectory.points[-1].positions[:]
moveit_start_state = RobotState()
moveit_start_state.joint_state = start_state
pub_display_msg = DisplayTrajectory()
pub_display_msg.model_id = "sia5"
pub_display_msg.trajectory.append(plan)
pub_display_msg.trajectory_start = moveit_start_state
self.display_hp_pub.publish(pub_display_msg)
# Send Action and Wait result
goal = FollowJointTrajectoryGoal(trajectory=plan.joint_trajectory)
rospy.loginfo("Start Task")
self.client.send_goal(goal)
self.client.wait_for_result()
# Grasping
if self.grasp_[0] != self.grasp_[1]:
self.executeGrasp(self.grasp_[0])
self.grasp_[1] = self.grasp_[0]
# Update the task queue
self.task_q.pop(0)
rospy.loginfo("End Task")
self.grasp_[0] = self.task_q[0].grasp
# Display the Trajectory
start_state = JointState()
start_state.header = Header()
start_state.header.stamp = rospy.Time.now()
start_state.name = plan.joint_trajectory.joint_names[:]
start_state.position = plan.joint_trajectory.points[-1].positions[:]
moveit_start_state = RobotState()
<<<<<<< HEAD
moveit_start_state.joint_state = start_state
=======
moveit_start_state.joint_state = start_state
>>>>>>> d95c7577c838d4cc104407dcb9fe5ba08262d4e9
pub_display_msg = DisplayTrajectory()
pub_display_msg.model_id = "sia5"
pub_display_msg.trajectory.append(plan)
pub_display_msg.trajectory_start = moveit_start_state
self.display_hp_pub.publish(pub_display_msg)
# Send Action and Wait result
goal = FollowJointTrajectoryGoal(trajectory=plan.joint_trajectory)
rospy.loginfo("Start Task")
self.client.send_goal(goal)
self.client.wait_for_result()
# Grasping
if self.grasp_[0] != self.grasp_[1]:
self.executeGrasp(self.grasp_[0])
self.grasp_[1] = self.grasp_[0]
