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 = 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
def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, group=None, options=None):
"""Calculates a cartesian motion path (linear in tool space).
Parameters
----------
robot : :class:`compas_fab.robots.Robot`
The robot instance for which the cartesian motion path is being calculated.
frames_WCF : list of :class:`compas.geometry.Frame`
The frames through which the path is defined.
start_configuration : :class:`Configuration`, optional
The robot's full configuration, i.e. values for all configurable
joints of the entire robot, at the starting position.
group : str, optional
The planning group used for calculation.
options : dict, optional
Dictionary containing kwargs for arguments specific to
the client being queried.
Returns
-------
:class:`compas_fab.robots.JointTrajectory`
The calculated trajectory.
Notes
-----
This will only work with robots that have 6 revolute joints.
"""
# what is the expected behaviour of that function?
# - Return all possible paths or select only the one that is closest to the start_configuration?
# - Do we use a stepsize to sample in between frames or use only the input frames?
# convert the frame WCF to RCF
base_frame = robot.get_base_frame(group=group, full_configuration=start_configuration)
frames_RCF = [base_frame.to_local_coordinates(frame_WCF) for frame_WCF in frames_WCF]
options = options or {}
options.update({"keep_order": True})
configurations_along_path = []
for frame in frames_RCF:
configurations = list(robot.iter_inverse_kinematics(frame, options=options))
configurations_along_path.append(configurations)
paths = []
for configurations in zip(*configurations_along_path):
if all(configurations):
paths.append(configurations)
if not len(paths):
raise CartesianMotionError("No complete trajectory found.")
# now select the path that is closest to the start configuration.
first_configurations = [path[0] for path in paths]
diffs = [sum([abs(d) for d in start_configuration.iter_differences(c)]) for c in first_configurations]
idx = argmin(diffs)
path = paths[idx]
path = self.smooth_configurations(path)
trajectory = JointTrajectory()
trajectory.fraction = len(path)/len(frames_RCF)
trajectory.joint_names = path[0].joint_names
trajectory.points = [JointTrajectoryPoint(config.joint_values, config.joint_types) for config in path]
trajectory.start_configuration = robot.merge_group_with_full_configuration(path[0], start_configuration, group)
return trajectory
