def set_robot_pose(self, robot, frame):
"""Moves the robot to a given pose, specified as a frame.
Args:
robot (:class:`compas_fab.robots.Robot`): Robot instance to move.
frame (:class:`Frame`): Target or goal frame.
Returns:
An instance of :class:`Configuration` found for the given pose.
"""
assert_robot(robot)
# First check if the start state is reachable
joints = len(robot.get_configurable_joints())
options = {
'num_joints': joints,
'metric_values': [0.] * joints,
}
joint_values, joint_names = list(self.inverse_kinematics(robot, frame, group=robot.model.attr['index'], options=options))[-1]
config = config_from_vrep(joint_values, self.scale)
if not config:
raise ValueError('Cannot find a valid config for the given pose')
self.set_robot_config(robot, config)
return config
def inverse_kinematics(self,
robot,
frame_WCF,
start_configuration=None,
group=None,
options=None):
"""Calculates inverse kinematics to find valid robot configurations for the specified goal frame.
Args:
robot (:class:`compas_fab.robots.Robot`): The robot instance for which inverse kinematics is being calculated.
frame_WCF (:class:`Frame`): Target or goal frame.
start_configuration (:obj:`None`): Unused parameter.
group (:obj:`int`): Integer referencing the desired robot group.
options (:obj:`dict`): Dictionary containing the following key-value pairs:
- ``"num_joints"``: (:obj:`int`) Number of configurable joints.
- ``"metric_values"``: (:obj:`list` of :obj:`float`) List containing one value
per configurable joint. Each value ranges from 0 to 1,
where 1 indicates the axis/joint is blocked and cannot
move during inverse kinematic solving.
- ``"gantry_joint_limits"``; (:obj:`list` of :obj:`float`) List of 6 floats defining the upper/lower limits of
gantry joints. Use this if you want to restrict the area in which to search for states.
- ``"arm_joint_limits"``: (:obj:`list` of :obj:`float`) List of 12 floats defining the upper/lower limits of
arm joints. Use this if you want to restrict the working area in which to search for states.
- ``"max_trials"``: (:obj:`int`) Number of trials to run. Set to ``None``
to retry infinitely.
- ``"max_results"``: (:obj:`int`) Maximum number of result states to return.
Returns:
list: List of :class:`Configuration` objects representing
the collision-free configuration for the ``goal_frame``.
"""
options = options or {}
num_joints = options['num_joints']
metric_values = options.get('metric_values')
gantry_joint_limits = options.get('gantry_joint_limits')
arm_joint_limits = options.get('arm_joint_limits')
max_trials = options.get('max_trials')
max_results = options.get('max_results', 1)
if not metric_values:
metric_values = [0.1] * num_joints
self.client.set_robot_metric(group, metric_values)
states = self.client.find_raw_robot_states(
group, frame_to_vrep_pose(frame_WCF, self.client.scale),
gantry_joint_limits, arm_joint_limits, max_trials, max_results)
return [
config_from_vrep(states[i:i + num_joints], self.client.scale)
for i in range(0, len(states), num_joints)
]
def get_robot_config(self, robot):
"""Gets the current configuration of the specified robot.
Args:
robot (:class:`compas_fab.robots.Robot`): Robot instance.
Examples:
>>> from compas_fab.robots import *
>>> with VrepClient() as client:
... config = client.get_robot_config(rfl.Robot('A'))
Returns:
An instance of :class:`.Configuration`.
"""
assert_robot(robot)
_res, _, config, _, _ = self.run_child_script(
'getRobotState', [robot.model.attr['index']], [], [])
return config_from_vrep(config, self.scale)
def _find_path_plan(self, group, goal, num_joints, metric_values, collision_meshes,
planner_id, trials, resolution,
gantry_joint_limits, arm_joint_limits, shallow_state_search, optimize_path_length,
log):
if not metric_values:
metric_values = [0.1] * num_joints
if planner_id not in self.SUPPORTED_PLANNERS:
raise ValueError('Unsupported planner_id. Must be one of: ' + str(self.SUPPORTED_PLANNERS))
first_start = timer() if log else None
if collision_meshes:
self.client.add_collision_mesh(collision_meshes)
if log:
log.debug('Execution time: add_collision_mesh=%.2f', timer() - first_start)
start = timer() if log else None
self.client.set_robot_metric(group, metric_values)
if log:
log.debug('Execution time: set_robot_metric=%.2f', timer() - start)
if 'target_type' not in goal:
raise ValueError('Invalid goal type, you are using an internal function but passed incorrect args')
if goal['target_type'] == 'config':
states = []
for c in goal['target']:
states.extend(config_to_vrep(c, self.client.scale))
elif goal['target_type'] == 'pose':
start = timer() if log else None
max_trials = None if shallow_state_search else 80
max_results = 1 if shallow_state_search else 80
states = self.client.find_raw_robot_states(group, frame_to_vrep_pose(goal['target'], self.client.scale), gantry_joint_limits, arm_joint_limits, max_trials, max_results)
if log:
log.debug('Execution time: search_robot_states=%.2f', timer() - start)
start = timer() if log else None
string_param_list = [planner_id]
if gantry_joint_limits or arm_joint_limits:
joint_limits = []
joint_limits.extend(floats_to_vrep(gantry_joint_limits or [], self.client.scale))
joint_limits.extend(arm_joint_limits or [])
string_param_list.append(','.join(map(str, joint_limits)))
if log:
log.debug('About to execute path planner: planner_id=%s, trials=%d, shallow_state_search=%s, optimize_path_length=%s',
planner_id, trials, shallow_state_search, optimize_path_length)
res, _, path, _, _ = self.client.run_child_script('searchRobotPath',
[group,
trials,
(int)(resolution * 1000),
1 if optimize_path_length else 0],
states, string_param_list)
if log:
log.debug('Execution time: search_robot_path=%.2f', timer() - start)
if res != 0:
raise VrepError('Failed to search robot path', res)
if log:
log.debug('Execution time: total=%.2f', timer() - first_start)
return [config_from_vrep(path[i:i + num_joints], self.client.scale)
for i in range(0, len(path), num_joints)]