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 _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 reset_module(self):
"""
Reset all the module's parameters to their start values.
"""
self.increment = 0
self.new_command = 0
self.move_type = 0
self.start_pose = Pose()
self.goal_pose = Pose()
self.command = 0
self.executed = False
self.stamps = ""
self.pose_traj = PoseArray()
self.trajectory = RobotTrajectory()
rospy.loginfo("Trajectory successfully executed")
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 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 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 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 generate_trajectory(self, randomness=1e-10, duration=-1):
"""
Generate a new trajectory from the given demonstrations and parameters
:param duration: Desired duration, auto if duration < 0
:return: the generated RobotTrajectory message
"""
trajectory_array = self.promp.generate_trajectory(randomness)
rt = RobotTrajectory()
rt.joint_trajectory.joint_names = self.joint_names
duration = float(self.mean_duration) if duration < 0 else duration
for point_idx, point in enumerate(trajectory_array):
time = point_idx * duration / float(self.num_points)
jtp = JointTrajectoryPoint(positions=map(float, point),
time_from_start=Duration(time))
rt.joint_trajectory.points.append(jtp)
return rt
def __filtering_plan(self, plan):
if plan is None:
return None
new_plan = RobotTrajectory()
new_plan.joint_trajectory.header = plan.joint_trajectory.header
new_plan.joint_trajectory.joint_names = plan.joint_trajectory.joint_names
new_plan.joint_trajectory.points = []
for i, point in enumerate(plan.joint_trajectory.points[:-1]):
if point.time_from_start != plan.joint_trajectory.points[
i + 1].time_from_start:
new_plan.joint_trajectory.points.append(point)
new_plan.joint_trajectory.points.append(
plan.joint_trajectory.points[-1])
return new_plan
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.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 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 get_plan(self, trans, z_offset, 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.target_pose.position.x = trans.transform.translation.x
self.target_pose.position.y = trans.transform.translation.y
self.target_pose.position.z = trans.transform.translation.z + z_offset
q = (trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w)
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(q)
pitch += pi / 2.0
tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
self.target_pose.orientation.x = tar_q[0]
self.target_pose.orientation.y = tar_q[1]
self.target_pose.orientation.z = tar_q[2]
self.target_pose.orientation.w = tar_q[3]
self.arm.set_pose_target(self.target_pose)
# 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 plan !!")
# publish the plan
pub_msg = HandringPlan()
pub_msg.grasp = grasp
print("---------debug--------{}".format(
len(plan.joint_trajectory.points)))
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)
def __init__(self, Kp, Ki, Kd, Kw, limb):
"""
Constructor:
Inputs:
Kp: 7x' ndarray of proportional constants
Ki: 7x' ndarray of integral constants
Kd: 7x' ndarray of derivative constants
Kw: 7x' ndarray of antiwindup constants
limb: baxter_interface.Limb or sawyer_interface.Limb
"""
# If the node is shutdown, call this function
rospy.on_shutdown(self.shutdown)
self._Kp = Kp
self._Ki = Ki
self._Kd = Kd
self._Kw = Kw
self._LastError = np.zeros(len(Kd))
self._LastTime = 0
self._IntError = np.zeros(len(Ki))
self._path = RobotTrajectory()
self._curIndex = 0
self._maxIndex = 0
self._limb = limb
# For Plotting:
self._times = list()
self._actual_positions = list()
self._actual_velocities = list()
self._target_positions = list()
self._target_velocities = list()
self._sensor_count = 0
self._sensor_data = list()
self._sensor_last = 0
self._velocity_scalar = 0.4
self._scalar_data = list()
self._goal_list = list()
self._delta = 0
self._trigger = 0
self._flag = 0
self._zero = 0
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 __init__(self, name):
self._action_name = name
self._server = actionlib.SimpleActionServer(
self._action_name,
ow_lander.msg.DeliverAction,
execute_cb=self.on_deliver_action,
auto_start=False)
self._server.start()
# Action Feedback/Result
self._fdbk = ow_lander.msg.UnstowFeedback()
self._result = ow_lander.msg.UnstowResult()
self._current_link_state = LinkStateSubscriber()
self._interface = MoveItInterface()
self._timeout = 0.0
self.trajectory_async_executer = TrajectoryAsyncExecuter()
self.trajectory_async_executer.connect("arm_controller")
self.deliver_sample_traj = RobotTrajectory()
def compute_cartisian_trajectory(self,way_points):
'''compute the trajectory according to the compute cartisian path api.inputs are waypoints
'''
trajectory=RobotTrajectory()
attempts=0
max_attempts=10000
fraction=0
while fraction<1 and attempts<max_attempts:
trajectory,fraction=self.group.compute_cartesian_path(way_points,0.01,0)
attempts+=1
if attempts % 10 == 0:
rospy.loginfo('still trying to solve the path after %d appempts',attempts)
if fraction == 1:
rospy.loginfo('the cartisian trajectory computed')
return trajectory
else:
rospy.logerr('cannot solve the path 100%,only' +str(fraction*100)+'% planned')
exit()
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 _to_trajectory_msg(self, traj, max_joint_vel=0.2):
""" Converts to a moveit_msgs/RobotTrajectory message """
msg = RobotTrajectory()
msg.joint_trajectory.joint_names = self.joint_names
t = 0.03
for i in range(traj.shape[0]):
p = JointTrajectoryPoint()
p.positions = traj[i, :].tolist()
p.positions[
2] -= math.pi # TODO this seems to be a bug in OpenRAVE?
p.time_from_start = rospy.Duration(t)
t += self.dt
msg.joint_trajectory.points.append(p)
#self._assign_constant_velocity_profile(msg, max_joint_vel)
return msg
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 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 robot_traj_generate(filename):
""" generate a message of RobotTrajectory type using read_goal_traj and return as "path" """
[traj_t, traj_q] = read_goal_traj(filename)
path = RobotTrajectory()
path.joint_trajectory.header.frame_id = "/world"
path.joint_trajectory.joint_names = JOINT_NAMES
path.joint_trajectory.points = [
JointTrajectoryPoint(positions=traj_q[1, :],
velocities=[0] * 6,
time_from_start=rospy.Duration(0.0))
]
d = .001
for i in range(traj_q.shape[0]):
path.joint_trajectory.points.append(
JointTrajectoryPoint(positions=traj_q[i, :],
velocities=[0] * 6,
time_from_start=rospy.Duration(d)))
d += .001 # seconds between each point
return path
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 __z_to_joint_traj(self, z, v, t):
"""
Converts a list of z joint values to a suitable RobotTrajectory over all
the joints. It keeps for the other joints the current joint value.
"""
import genpy
traj = RobotTrajectory()
traj.joint_trajectory.header.stamp = rospy.Time.now()
traj.joint_trajectory.joint_names = ["X", "Y", "B", "Z", "A"]
x, y, b, _, a = self.group.get_current_joint_values()
for zi, vi, ti in zip(z, v, t):
pt = JointTrajectoryPoint()
pt.positions = [x, y, b, zi, a]
pt.velocities = [0, 0, 0, vi, 0]
# pt.time_from_start = rospy.Time.from_sec(ti+MOTION_START_DELAY) # shift all
pt.time_from_start = rospy.Duration.from_sec(
ti + MOTION_START_DELAY) # shift all
# trajectory points in time to avoid dropping the first point due to its time being
traj.joint_trajectory.points.append(pt)
return traj
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 get_home_plan(self, 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
init_pose = self.arm.get_current_joint_values()
init_pose[0] = 0.0
init_pose[1] = 0.0
init_pose[2] = 0.0
init_pose[3] = 0.0
init_pose[4] = 0.0
init_pose[5] = 0.0
init_pose[6] = 0.0
self.arm.set_joint_value_target(init_pose)
# 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 home 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)
def start_learn(self,
robot_traj,
motion_name,
joints=[],
bag_name="no_bag_name_set"):
"""Start the learning writting in the accumulator of msgs from the RobotTrajectory msg"""
self.current_rosbag_name = bag_name
self.start_recording = True
if len(joints) > 0:
self.joints_to_record = joints
else:
rospy.logerr("No joints provided to record, aborting")
return
rt = RobotTrajectory()
for point in robot_traj.joint_trajectory.points:
js = JointState()
js.name = robot_traj.joint_trajectory.joint_names
js.header.stamp = point.time_from_start
js.position = point.positions
self.joint_states_accumulator.append(js)
def move_group_interface():
# First initialize moveit_commander and rospy.
moveit_commander.roscpp_initialize(sys.argv)
# Instantiate a MoveGroupCommander object. This object is an interface to one group of joints.
group = moveit_commander.MoveGroupCommander("quad_base")
#set workspace for the planner
group.set_workspace([-3.5,-3.5,0,3.5,3.5,3])
# set the goal and plan
group.set_joint_value_target(goal)
plan = group.plan()
#if plan is empty, tell FBETServer the goal is invalid
empty = RobotTrajectory()
if plan == empty:
print "Empty Plan"
feedback = String()
feedback.data = ""
pub_feedback.publish(feedback)
def connect_4(self, prm, q1, q2, only_transit=False):
# type: (RoadMap, Vertex, Vertex) -> (bool, RobotTrajectory)
assert isinstance(prm, RoadMap)
if only_transit:
type_q1 = prm.get_type_for_vertex(q1)
type_q2 = prm.get_type_for_vertex(q2)
if type_q1 != "travel" and type_q2 != "travel":
return False, RobotTrajectory()
q1_robot_state = prm.get_robot_state_for_vertex(q1)
q2_robot_state = prm.get_robot_state_for_vertex(q2)
mc = self.get_mgroup(prm.get_group_name())
mc.set_start_state(q1_robot_state)
plan = mc.plan(q2_robot_state.joint_state)
if len(plan.joint_trajectory.points) >= 2:
return True, plan
else:
return False, plan
