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 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 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 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
