def convert_to_trajectory(response):
trajectory = JointTrajectory()
trajectory.source_message = response
trajectory.fraction = 1.
trajectory.joint_names = response.trajectory.joint_trajectory.joint_names
trajectory.planning_time = response.planning_time
joint_types = [
joint_type_by_name[name] for name in trajectory.joint_names
]
trajectory.points = convert_trajectory_points(
response.trajectory.joint_trajectory.points, joint_types)
start_state = response.trajectory_start.joint_state
start_state_types = [
joint_type_by_name[name] for name in start_state.name
]
trajectory.start_configuration = Configuration(
start_state.position, start_state_types, start_state.name)
trajectory.attached_collision_meshes = list(
itertools.chain(*[
aco.to_attached_collision_meshes() for aco in
response.trajectory_start.attached_collision_objects
]))
callback(trajectory)
def convert_to_trajectory(response):
trajectory = JointTrajectory()
trajectory.source_message = response
trajectory.fraction = response.fraction
trajectory.points = convert_trajectory_points(
response.solution.joint_trajectory.points, joint_types)
trajectory.start_configuration = Configuration(
response.start_state.joint_state.position,
start_configuration.types)
callback(trajectory)
def convert_to_trajectory(response):
trajectory = JointTrajectory()
trajectory.source_message = response
trajectory.fraction = 1.
trajectory.points = convert_trajectory_points(
response.trajectory.joint_trajectory.points, joint_types)
trajectory.start_configuration = Configuration(
response.trajectory_start.joint_state.position,
start_configuration.types)
trajectory.planning_time = response.planning_time
callback(trajectory)
def convert_to_trajectory(response):
trajectory = JointTrajectory()
trajectory.source_message = response
trajectory.fraction = 1.
trajectory.joint_names = response.trajectory.joint_trajectory.joint_names
trajectory.planning_time = response.planning_time
joint_types = robot.get_joint_types_by_names(trajectory.joint_names)
trajectory.points = convert_trajectory_points(
response.trajectory.joint_trajectory.points, joint_types)
start_state = response.trajectory_start.joint_state
start_state_types = robot.get_joint_types_by_names(start_state.name)
trajectory.start_configuration = Configuration(start_state.position, start_state_types)
callback(trajectory)
def convert_to_trajectory(response):
try:
trajectory = JointTrajectory()
trajectory.source_message = response
trajectory.fraction = response.fraction
trajectory.joint_names = response.solution.joint_trajectory.joint_names
joint_types = [joint_type_by_name[name] for name in trajectory.joint_names]
trajectory.points = convert_trajectory_points(
response.solution.joint_trajectory.points, joint_types)
start_state = response.start_state.joint_state
start_state_types = [joint_type_by_name[name] for name in start_state.name]
trajectory.start_configuration = Configuration(start_state.position, start_state_types, start_state.name)
callback(trajectory)
except Exception as e:
errback(e)
def convert_trajectory(joints, solution, solution_start_state, fraction,
planning_time, source_message):
trajectory = JointTrajectory()
trajectory.source_message = source_message
trajectory.fraction = fraction
trajectory.joint_names = solution.joint_trajectory.joint_names
trajectory.planning_time = planning_time
joint_types = [joints[name] for name in trajectory.joint_names]
trajectory.points = convert_trajectory_points(
solution.joint_trajectory.points, joint_types)
start_state = solution_start_state.joint_state
start_state_types = [joints[name] for name in start_state.name]
trajectory.start_configuration = Configuration(start_state.position,
start_state_types,
start_state.name)
trajectory.attached_collision_meshes = list(
itertools.chain(*[
aco.to_attached_collision_meshes()
for aco in solution_start_state.attached_collision_objects
]))
return trajectory