def test_config_merge():
config = Configuration(values=[1, 2, 3],
types=[Joint.REVOLUTE] * 3,
joint_names=['a', 'b', 'c'])
other_config = Configuration(values=[3, 2, 0],
types=[Joint.REVOLUTE] * 3,
joint_names=['a', 'b', 'd'])
config.merge(other_config)
assert config.joint_dict == {'a': 3, 'b': 2, 'c': 3, 'd': 0}
def to_rlf_robot_full_conf(robot11_confval, robot12_confval, scale=1e-3):
# convert to full configuration of the RFL robot
robot21_confval = [38000, 0, -4915, 0, 0, 0, 0, 0, 0]
robot22_confval = [-12237, -4915, 0, 0, 0, 0, 0, 0]
return Configuration(
values=(
*[robot11_confval[i] * scale for i in range(0, 3)],
*[rad(robot11_confval[i]) for i in range(3, 9)],
*[robot12_confval[i] * scale for i in range(0, 2)],
*[rad(robot12_confval[i]) for i in range(2, 8)],
# below fixed
*[robot21_confval[i] * 1e-3 for i in range(0, 3)],
*[rad(robot21_confval[i]) for i in range(3, 9)],
*[robot22_confval[i] * 1e-3 for i in range(0, 2)],
*[rad(robot22_confval[i]) for i in range(2, 8)],
),
types=(2, 2, 2, 0, 0, 0, 0, 0, 0, 2, 2, 0, 0, 0, 0, 0, 0, 2, 2, 2, 0,
0, 0, 0, 0, 0, 2, 2, 0, 0, 0, 0, 0, 0),
joint_names=('bridge1_joint_EA_X', 'robot11_joint_EA_Y',
'robot11_joint_EA_Z', 'robot11_joint_1',
'robot11_joint_2', 'robot11_joint_3', 'robot11_joint_4',
'robot11_joint_5', 'robot11_joint_6',
'robot12_joint_EA_Y', 'robot12_joint_EA_Z',
'robot12_joint_1', 'robot12_joint_2', 'robot12_joint_3',
'robot12_joint_4', 'robot12_joint_5', 'robot12_joint_6',
'bridge2_joint_EA_X', 'robot21_joint_EA_Y',
'robot21_joint_EA_Z', 'robot21_joint_1',
'robot21_joint_2', 'robot21_joint_3', 'robot21_joint_4',
'robot21_joint_5', 'robot21_joint_6',
'robot22_joint_EA_Y', 'robot22_joint_EA_Z',
'robot22_joint_1', 'robot22_joint_2', 'robot22_joint_3',
'robot22_joint_4', 'robot22_joint_5', 'robot22_joint_6'))
def test_scale():
values = [1.5, 1., 2.]
types = [Joint.REVOLUTE, Joint.PRISMATIC, Joint.PLANAR]
config = Configuration(values, types)
config.scale(1000.)
assert config.values == [1.5, 1000., 2000.]
def plan_moving_and_placing_motion(robot, element, start_configuration, tolerance_vector, safelevel_vector, attached_element_mesh):
"""Returns two trajectories for moving and placing an element.
Parameters
----------
robot : :class:`compas.robots.Robot`
element : :class:`Element`
start_configuration : :class:`Configuration`
tolerance_vector : :class:`Vector`
safelevel_vector : :class:`Vector`
attached_element_mesh : :class:`AttachedCollisionMesh`
Returns
-------
list of :class:`JointTrajectory`
"""
tolerance_position = 0.001
tolerance_axes = [math.radians(1)] * 3
target_frame = element._tool_frame.copy()
target_frame.point += tolerance_vector
safelevel_target_frame = target_frame.copy()
safelevel_target_frame.point += safelevel_vector
# Calculate goal constraints
safelevel_target_frame_tool0 = robot.from_tcf_to_t0cf(
[safelevel_target_frame])[0]
goal_constraints = robot.constraints_from_frame(safelevel_target_frame_tool0,
tolerance_position,
tolerance_axes)
moving_trajectory = robot.plan_motion(goal_constraints,
start_configuration,
options=dict(
planner_id='SBL',
attached_collision_meshes=[attached_element_mesh],
num_planning_attempts=20,
allowed_planning_time=10
))
frames = [safelevel_target_frame, target_frame]
frames_tool0 = robot.from_tcf_to_t0cf(frames)
# as start configuration take last trajectory's end configuration
last_configuration = Configuration(moving_trajectory.points[-1].values, moving_trajectory.points[-1].types)
placing_trajectory = robot.plan_cartesian_motion(frames_tool0,
last_configuration,
options=dict(
max_step=0.01,
attached_collision_meshes=[attached_element_mesh]
))
return moving_trajectory, placing_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 test_cast_to_str():
config = Configuration([pi / 2, 3., 0.1],
[Joint.REVOLUTE, Joint.PRISMATIC, Joint.PLANAR])
assert str(config) == 'Configuration((1.571, 3.000, 0.100), (0, 2, 5))'
def test_collision_checker(rfl_setup, itj_beam_cm, viewer, diagnosis):
# modified from https://github.com/yijiangh/pybullet_planning/blob/dev/tests/test_collisions.py
urdf_filename, semantics = rfl_setup
move_group = 'robot11_eaXYZ' # 'robot12_eaYZ'
with PyChoreoClient(viewer=viewer) as client:
cprint(urdf_filename, 'green')
with LockRenderer():
robot = client.load_robot(urdf_filename)
robot.semantics = semantics
robot_uid = client.get_robot_pybullet_uid(robot)
ik_joint_names = robot.get_configurable_joint_names(group=move_group)
ik_joint_types = robot.get_joint_types_by_names(ik_joint_names)
flange_link_name = robot.get_end_effector_link_name(group=move_group)
ee_touched_link_names = ['robot12_tool0', 'robot12_link_6']
# ee_acms = [AttachedCollisionMesh(ee_cm, flange_link_name, ee_touched_link_names) for ee_cm in itj_TC_PG500_cms]
# beam_acm = AttachedCollisionMesh(itj_beam_cm, flange_link_name, ee_touched_link_names)
draw_pose(unit_pose(), length=1.)
cam = rfl_camera()
set_camera_pose(cam['target'], cam['location'])
ik_joints = joints_from_names(robot_uid, ik_joint_names)
flange_link = link_from_name(robot_uid, flange_link_name)
# robot_base_link = link_from_name(robot_uid, base_link_name)
r11_start_conf_vals = np.array([22700.0, 0.0, -4900.0, \
0.0, -80.0, 65.0, 65.0, 20.0, -20.0])
r12_start_conf_vals = np.array([-4056.0883789999998, -4000.8486330000001, \
0.0, -22.834741999999999, -30.711554, 0.0, 57.335655000000003, 0.0])
full_start_conf = to_rlf_robot_full_conf(r11_start_conf_vals,
r12_start_conf_vals)
# full_joints = joints_from_names(robot_uid, full_start_conf.joint_names)
client.set_robot_configuration(robot, full_start_conf)
# # * add attachment
# for ee_acm in ee_acms:
# client.add_attached_collision_mesh(ee_acm, options={'robot' : robot, 'mass': 1})
# client.add_attached_collision_mesh(beam_acm, options={'robot' : robot, 'mass': 1})
# safe start conf
start_confval = np.hstack([
r11_start_conf_vals[:3] * 1e-3,
np.radians(r11_start_conf_vals[3:])
])
conf = Configuration(values=start_confval.tolist(),
types=ik_joint_types,
joint_names=ik_joint_names)
assert not client.check_collisions(
robot, conf, options={'diagnosis': diagnosis})
# TODO add more tests
from pybullet_planning import get_link_subtree, prune_fixed_joints, clone_body, get_movable_joints, get_all_links, \
set_color, child_link_from_joint
first_joint = ik_joints[0]
target_link = flange_link
# selected_links = get_link_subtree(robot_uid, first_joint) # TODO: child_link_from_joint?
selected_links = [child_link_from_joint(joint) for joint in ik_joints]
selected_movable_joints = prune_fixed_joints(robot_uid, selected_links)
assert (target_link in selected_links)
selected_target_link = selected_links.index(target_link)
sub_robot = clone_body(robot_uid,
links=selected_links,
visual=True,
collision=True) # TODO: joint limits
sub_movable_joints = get_movable_joints(sub_robot)
# conf = Configuration(values=[0.]*6, types=ik_joint_types, joint_names=ik_joint_names)
# assert not client.configuration_in_collision(conf, group=move_group, options={'diagnosis':True})
wait_if_gui()
def test_ctor():
values = [1., 3., 0.1]
types = [Joint.REVOLUTE, Joint.PRISMATIC, Joint.PLANAR]
config = Configuration(values, types)
assert config.values == values
assert config.types == types
from math import pi
from compas.robots import Joint
from compas_fab.robots import Configuration
print(Joint.REVOLUTE)
print(Joint.PRISMATIC)
print(Joint.FIXED)
values = [0] * 6
types = [Joint.REVOLUTE] * 6
config = Configuration(values, types)
config = Configuration([pi / 2, 3., 0.1],
[Joint.REVOLUTE, Joint.PRISMATIC, Joint.PLANAR])
config = Configuration.from_revolute_values([pi / 2, 0., 0., pi / 2, pi, 0])
config = Configuration.from_prismatic_and_revolute_values(
[8.312], [pi / 2, 0., 0., 0., 2 * pi, 0.8])
def test_plan_motion(abb_irb4600_40_255_setup, itj_TC_g1_cms, itj_beam_cm,
column_obstacle_cm, base_plate_cm,
itj_tool_changer_grasp_transf, itj_gripper_grasp_transf,
itj_beam_grasp_transf, tool_type,
itj_tool_changer_urdf_path, itj_g1_urdf_path, viewer,
diagnosis):
# modified from https://github.com/yijiangh/pybullet_planning/blob/dev/tests/test_collisions.py
urdf_filename, semantics = abb_irb4600_40_255_setup
move_group = 'bare_arm'
ee_touched_link_names = ['link_6']
with PyChoreoClient(viewer=viewer) as client:
with LockRenderer():
robot = client.load_robot(urdf_filename)
robot.semantics = semantics
client.disabled_collisions = robot.semantics.disabled_collisions
if tool_type == 'static':
for _, ee_cm in itj_TC_g1_cms.items():
client.add_collision_mesh(ee_cm)
else:
client.add_tool_from_urdf('TC', itj_tool_changer_urdf_path)
client.add_tool_from_urdf('g1', itj_g1_urdf_path)
# * add static obstacles
client.add_collision_mesh(base_plate_cm)
client.add_collision_mesh(column_obstacle_cm)
ik_joint_names = robot.get_configurable_joint_names(group=move_group)
ik_joint_types = robot.get_joint_types_by_names(ik_joint_names)
flange_link_name = robot.get_end_effector_link_name(group=move_group)
tool0_tf = Transformation.from_frame(
client.get_link_frame_from_name(robot, flange_link_name))
tool0_from_tool_changer_base = itj_tool_changer_grasp_transf
tool0_from_gripper_base = itj_gripper_grasp_transf
client.set_object_frame(
'^{}'.format('TC'),
Frame.from_transformation(tool0_tf * tool0_from_tool_changer_base))
client.set_object_frame(
'^{}'.format('g1'),
Frame.from_transformation(tool0_tf * tool0_from_gripper_base))
names = client._get_collision_object_names('^{}'.format('g1')) + \
client._get_collision_object_names('^{}'.format('TC'))
for ee_name in names:
attach_options = {'robot': robot}
if tool_type == 'actuated':
attached_child_link_name = 'toolchanger_base' if 'TC' in ee_name else 'gripper_base'
attach_options.update(
{'attached_child_link_name': attached_child_link_name})
client.add_attached_collision_mesh(AttachedCollisionMesh(
CollisionMesh(None, ee_name),
flange_link_name,
touch_links=ee_touched_link_names),
options=attach_options)
#* attach beam
client.add_collision_mesh(itj_beam_cm)
tool0_tf = Transformation.from_frame(
client.get_link_frame_from_name(robot, flange_link_name))
tool0_from_beam_base = itj_beam_grasp_transf
client.set_object_frame(
'^{}$'.format('itj_beam_b2'),
Frame.from_transformation(tool0_tf * tool0_from_beam_base))
client.add_attached_collision_mesh(AttachedCollisionMesh(
CollisionMesh(None, 'itj_beam_b2'),
flange_link_name,
touch_links=[]),
options={'robot': robot})
wait_if_gui('beam attached.')
vals = [
-1.4660765716752369, -0.22689280275926285, 0.27925268031909273,
0.17453292519943295, 0.22689280275926285, -0.22689280275926285
]
start_conf = Configuration(values=vals,
types=ik_joint_types,
joint_names=ik_joint_names)
# client.set_robot_configuration(robot, start_conf)
# wait_if_gui()
# vals = [0.05235987755982989, -0.087266462599716474, -0.05235987755982989, 1.7104226669544429, 0.13962634015954636, -0.43633231299858238]
vals = [
0.034906585039886591, 0.68067840827778847, 0.15707963267948966,
-0.89011791851710809, -0.034906585039886591, -2.2514747350726849
]
end_conf = Configuration(values=vals,
types=ik_joint_types,
joint_names=ik_joint_names)
# client.set_robot_configuration(robot, end_conf)
# wait_if_gui()
plan_options = {'diagnosis': True, 'resolutions': 0.05}
goal_constraints = robot.constraints_from_configuration(
end_conf, [0.01], [0.01], group=move_group)
st_time = time.time()
trajectory = client.plan_motion(robot,
goal_constraints,
start_configuration=start_conf,
group=move_group,
options=plan_options)
print('Solving time: {}'.format(elapsed_time(st_time)))
if trajectory is None:
cprint('Client motion planner CANNOT find a plan!', 'red')
# assert False, 'Client motion planner CANNOT find a plan!'
# TODO warning
else:
cprint('Client motion planning find a plan!', 'green')
wait_if_gui('Start sim.')
time_step = 0.03
for traj_pt in trajectory.points:
client.set_robot_configuration(robot, traj_pt)
wait_for_duration(time_step)
wait_if_gui("Finished.")
))
assert (trajectory1.fraction == 1.)
# ==========================================================================
key = 0
element = assembly.element(key)
print("Calculating free-space path to safe vector above target frame...")
# 2. Calulate a free-space motion to the safelevel_target_frame
safelevel_target_frame_tool0 = robot.from_tcf_to_t0cf(
[safelevel_target_frame])[0]
goal_constraints = robot.constraints_from_frame(
safelevel_target_frame_tool0, tolerance_position, tolerance_axes)
# as start configuration take last trajectory's end configuration
start_configuration = Configuration(trajectory1.points[-1].values,
trajectory1.points[-1].types)
trajectory2 = robot.plan_motion(goal_constraints,
start_configuration,
options=dict(
planner_id='RRT',
attached_collision_meshes=[brick_acm]))
# 3. Calculate a cartesian motion to the target_frame
print("Calculating cartesian path for placement...")
frames = [safelevel_target_frame, target_frame]
# as start configuration take last trajectory's end configuration
start_configuration = Configuration(trajectory2.points[-1].values,
trajectory2.points[-1].types)
trajectory3 = robot.plan_cartesian_motion(
robot.from_tcf_to_t0cf(frames),
start_configuration,
def test_collision_checker(abb_irb4600_40_255_setup, itj_TC_g1_cms,
itj_beam_cm, column_obstacle_cm, base_plate_cm,
itj_tool_changer_grasp_transf,
itj_gripper_grasp_transf, itj_beam_grasp_transf,
tool_type, itj_tool_changer_urdf_path,
itj_g1_urdf_path, viewer, diagnosis):
# modified from https://github.com/yijiangh/pybullet_planning/blob/dev/tests/test_collisions.py
urdf_filename, semantics = abb_irb4600_40_255_setup
move_group = 'bare_arm'
ee_touched_link_names = ['link_6']
with PyChoreoClient(viewer=viewer) as client:
with LockRenderer():
robot = client.load_robot(urdf_filename)
robot.semantics = semantics
client.disabled_collisions = robot.semantics.disabled_collisions
if tool_type == 'static':
for _, ee_cm in itj_TC_g1_cms.items():
client.add_collision_mesh(ee_cm)
else:
client.add_tool_from_urdf('TC', itj_tool_changer_urdf_path)
client.add_tool_from_urdf('g1', itj_g1_urdf_path)
# * add static obstacles
client.add_collision_mesh(base_plate_cm)
client.add_collision_mesh(column_obstacle_cm)
ik_joint_names = robot.get_configurable_joint_names(group=move_group)
ik_joint_types = robot.get_joint_types_by_names(ik_joint_names)
flange_link_name = robot.get_end_effector_link_name(group=move_group)
tool0_tf = Transformation.from_frame(
client.get_link_frame_from_name(robot, flange_link_name))
tool0_from_tool_changer_base = itj_tool_changer_grasp_transf
tool0_from_gripper_base = itj_gripper_grasp_transf
client.set_object_frame(
'^{}'.format('TC'),
Frame.from_transformation(tool0_tf * tool0_from_tool_changer_base))
client.set_object_frame(
'^{}'.format('g1'),
Frame.from_transformation(tool0_tf * tool0_from_gripper_base))
names = client._get_collision_object_names('^{}'.format('g1')) + \
client._get_collision_object_names('^{}'.format('TC'))
for ee_name in names:
attach_options = {'robot': robot}
if tool_type == 'actuated':
attached_child_link_name = 'toolchanger_base' if 'TC' in ee_name else 'gripper_base'
attach_options.update(
{'attached_child_link_name': attached_child_link_name})
client.add_attached_collision_mesh(AttachedCollisionMesh(
CollisionMesh(None, ee_name),
flange_link_name,
touch_links=ee_touched_link_names),
options=attach_options)
# client._print_object_summary()
# wait_if_gui('EE attached.')
if tool_type == 'actuated':
# lower 0.0008 upper 0.01
tool_bodies = client._get_bodies('^{}'.format('itj_PG500'))
tool_conf = Configuration(
values=[0.01, 0.01],
types=[Joint.PRISMATIC, Joint.PRISMATIC],
joint_names=['joint_gripper_jaw_l', 'joint_gripper_jaw_r'])
for b in tool_bodies:
client._set_body_configuration(b, tool_conf)
wait_if_gui('Open')
tool_conf = Configuration(
values=[0.0008, 0.0008],
types=[Joint.PRISMATIC, Joint.PRISMATIC],
joint_names=['joint_gripper_jaw_l', 'joint_gripper_jaw_r'])
for b in tool_bodies:
client._set_body_configuration(b, tool_conf)
wait_if_gui('Close')
cprint('safe start conf', 'green')
conf = Configuration(values=[0.] * 6,
types=ik_joint_types,
joint_names=ik_joint_names)
assert not client.check_collisions(
robot, conf, options={'diagnosis': diagnosis})
cprint('joint over limit', 'cyan')
conf = Configuration(values=[0., 0., 1.5, 0, 0, 0],
types=ik_joint_types,
joint_names=ik_joint_names)
assert client.check_collisions(robot,
conf,
options={'diagnosis': diagnosis})
cprint('attached gripper-obstacle collision - column', 'cyan')
vals = [
-0.33161255787892263, -0.43633231299858238, 0.43633231299858238,
-1.0471975511965976, 0.087266462599716474, 0.0
]
# conf = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names)
# client.set_robot_configuration(robot, conf)
# wait_if_gui()
assert client.check_collisions(robot,
conf,
options={'diagnosis': diagnosis})
#* attach beam
client.add_collision_mesh(itj_beam_cm)
tool0_tf = Transformation.from_frame(
client.get_link_frame_from_name(robot, flange_link_name))
tool0_from_beam_base = itj_beam_grasp_transf
client.set_object_frame(
'^{}$'.format('itj_beam_b2'),
Frame.from_transformation(tool0_tf * tool0_from_beam_base))
client.add_attached_collision_mesh(AttachedCollisionMesh(
CollisionMesh(None, 'itj_beam_b2'),
flange_link_name,
touch_links=[]),
options={'robot': robot})
# wait_if_gui('beam attached.')
cprint('attached beam-robot body self collision', 'cyan')
vals = [
0.73303828583761843, -0.59341194567807209, 0.54105206811824214,
-0.17453292519943295, 1.064650843716541, 1.7278759594743862
]
conf = Configuration(values=vals,
types=ik_joint_types,
joint_names=ik_joint_names)
assert client.check_collisions(robot,
conf,
options={'diagnosis': diagnosis})
cprint('attached beam-obstacle collision - column', 'cyan')
vals = [
0.087266462599716474, -0.19198621771937624, 0.20943951023931956,
0.069813170079773182, 1.2740903539558606, 0.069813170079773182
]
conf = Configuration(values=vals,
types=ik_joint_types,
joint_names=ik_joint_names)
assert client.check_collisions(robot,
conf,
options={'diagnosis': diagnosis})
cprint('attached beam-obstacle collision - ground', 'cyan')
vals = [
-0.017453292519943295, 0.6108652381980153, 0.20943951023931956,
1.7627825445142729, 1.2740903539558606, 0.069813170079773182
]
conf = Configuration(values=vals,
types=ik_joint_types,
joint_names=ik_joint_names)
assert client.check_collisions(robot,
conf,
options={'diagnosis': diagnosis})
cprint('robot link-obstacle collision - column', 'cyan')
vals = [
-0.41887902047863912, 0.20943951023931956, 0.20943951023931956,
1.7627825445142729, 1.2740903539558606, 0.069813170079773182
]
conf = Configuration(values=vals,
types=ik_joint_types,
joint_names=ik_joint_names)
assert client.check_collisions(robot,
conf,
options={'diagnosis': diagnosis})
cprint('robot link-obstacle collision - ground', 'cyan')
vals = [
0.33161255787892263, 1.4660765716752369, 0.27925268031909273,
0.17453292519943295, 0.22689280275926285, 0.54105206811824214
]
conf = Configuration(values=vals,
types=ik_joint_types,
joint_names=ik_joint_names)
assert client.check_collisions(robot,
conf,
options={'diagnosis': diagnosis})
cprint('Sweeping collision', 'cyan')
vals = [
-0.12217304763960307, -0.73303828583761843, 0.83775804095727824,
-2.4609142453120048, 1.2391837689159739, -0.85521133347722145
]
conf1 = Configuration(values=vals,
types=ik_joint_types,
joint_names=ik_joint_names)
assert not client.check_collisions(
robot, conf1, options={'diagnosis': diagnosis})
# wait_if_gui()
vals = [
-0.12217304763960307, -0.73303828583761843, 0.83775804095727824,
-2.4958208303518914, -1.5533430342749532, -0.85521133347722145
]
conf2 = Configuration(values=vals,
types=ik_joint_types,
joint_names=ik_joint_names)
assert not client.check_collisions(
robot, conf2, options={'diagnosis': diagnosis})
# wait_if_gui()
assert client.check_sweeping_collisions(robot,
conf1,
conf2,
options={
'diagnosis': diagnosis,
'line_width': 3.0
})
wait_if_gui("Finished.")
# Units:
# - Revolute joint : radiants
# - Prismatic joint: meters
import math
from compas.robots.model import Joint
from compas_fab.robots import Configuration
print('Default constructor')
print(Configuration([math.pi, 4], [Joint.REVOLUTE, Joint.PRISMATIC]))
print(
Configuration([math.pi, 4], [Joint.REVOLUTE, Joint.PRISMATIC],
['joint_1', 'ext_axis_1']))
print()
print('Construct from revolute values')
print(Configuration.from_revolute_values([math.pi, 0]))
print(Configuration.from_revolute_values([math.pi, 0], ['joint_1', 'joint_2']))
print()
print('Construct from prismatic & revolute values')
print(Configuration.from_prismatic_and_revolute_values([4], [math.pi]))
print(
Configuration.from_prismatic_and_revolute_values(
[4], [math.pi], ['ext_axis_1', 'joint_1']))
print()
print('Merge two configurations')
ext_axes = Configuration([4], [Joint.PRISMATIC], ['ext_axis_1'])
arm_joints = Configuration([math.pi], [Joint.REVOLUTE], ['joint_1'])
full_cfg = ext_axes.merged(arm_joints)
# create cylinder in yz plane
radius, length = 0.3, 5
cylinder = Cylinder(Circle(Plane([0, 0, 0], [1, 0, 0]), radius), length)
cylinder.transform(Translation([length / 2., 0, 0]))
mesh = Mesh.from_shape(cylinder)
# create robot
robot = RobotModel("robot", links=[], joints=[])
link0 = robot.add_link("world")
# Add all other links to robot
for count in range(1, 8):
# add new link to robot
robot.add_link("link" + str(count), visual_mesh=mesh.copy(), visual_color=(count * 0.72 % 1.0, count * 0.23 % 1.0 , 0.6))
# add the joint between the last two links
axis = (0, 0, 1)
origin = Frame((length , 0, 0), (1, 0, 0), (0, 1, 0))
robot.add_joint("joint" + str(count), Joint.CONTINUOUS, robot.links[-2] , robot.links[-1], origin, axis)
artist = RobotArtist(robot)
artist.clear_layer()
joint_names = robot.get_configurable_joint_names()
joint_values = [0.2] * len(joint_names)
joint_types = [Joint.CONTINUOUS] * len(joint_names)
config = Configuration(joint_values, joint_types)
artist.update(config, joint_names)
artist.draw_visual()
def inverse_kinematics(self,
robot,
frame_WCF,
start_configuration=None,
group=None,
options=None):
"""Calculate the robot's inverse kinematic for a given frame.
Parameters
----------
robot : :class:`compas_fab.robots.Robot`
The robot instance for which inverse kinematics is being calculated.
frame_WCF: :class:`compas.geometry.Frame`
The frame to calculate the inverse for.
start_configuration: :class:`compas_fab.robots.Configuration`, optional
If passed, the inverse will be calculated such that the calculated
joint positions differ the least from the start_configuration.
Defaults to the init configuration.
group: str, optional
The planning group used for calculation. Defaults to the robot's
main planning group.
options: dict, optional
Dictionary containing the following key-value pairs:
- ``"base_link"``: (:obj:`str`) Name of the base link.
- ``"avoid_collisions"``: (:obj:`bool`, optional) Whether or not to avoid collisions.
Defaults to `True`.
- ``"constraints"``: (:obj:`list` of :class:`compas_fab.robots.Constraint`, optional)
A set of constraints that the request must obey. Defaults to `None`.
- ``"attempts"``: (:obj:`int`, optional) The maximum number of inverse kinematic attempts.
Defaults to `8`.
- ``"attached_collision_meshes"``: (:obj:`list` of :class:`compas_fab.robots.AttachedCollisionMesh`, optional)
Defaults to `None`.
- ``"return_all"``: (bool, optional) return a list of all computed ik solutions
Defaults to False
Raises
------
compas_fab.backends.exceptions.BackendError
If no configuration can be found.
Returns
-------
:class:`compas_fab.robots.Configuration`
The planning group's configuration.
"""
max_iterations = is_valid_option(options, 'attempts', 8)
avoid_collisions = is_valid_option(options, 'avoid_collisions', True)
return_all = is_valid_option(options, 'return_all', False)
custom_limits = options.get('custom_limits') or {}
# return_closest_to_start = is_valid_option(options, 'return_closest_to_start', False)
# cull = is_valid_option(options, 'cull', True)
robot_uid = robot.attributes['pybullet_uid']
ik_joint_names = robot.get_configurable_joint_names(group=group)
ik_joints = joints_from_names(robot_uid, ik_joint_names)
tool_link_name = robot.get_end_effector_link_name(group=group)
tool_link = link_from_name(robot_uid, tool_link_name)
pb_custom_limits = get_custom_limits(
robot_uid,
ik_joints,
custom_limits={
joint_from_name(robot_uid, jn): lims
for jn, lims in custom_limits.items()
})
target_pose = pose_from_frame(frame_WCF)
with WorldSaver():
if start_configuration is not None:
start_conf_vals = start_configuration.values
set_joint_positions(robot_uid, ik_joints, start_conf_vals)
# else:
# sample_fn = get_sample_fn(robot_uid, ik_joints)
# start_conf_vals = sample_fn()
# if group not in self.client.planner.ik_fn_from_group:
# use default ik fn
conf_vals = self._compute_ik(robot_uid,
ik_joints,
tool_link,
target_pose,
max_iterations,
custom_limits=pb_custom_limits)
joint_types = robot.get_joint_types_by_names(ik_joint_names)
configurations = [Configuration(values=conf_val, types=joint_types, joint_names=ik_joint_names) \
for conf_val in conf_vals if conf_val is not None]
# else:
# # qs = client.inverse_kinematics(frame_WCF, group=move_group)
# configurations = self.client.planner.ik_fn_from_group[group](frame_WCF, group=group, options=options)
if avoid_collisions:
configurations = [
conf for conf in configurations
if not self.client.check_collisions(
robot, conf, options=options)
]
if return_all:
return configurations
else:
return configurations[0] if len(configurations) > 0 else None
# 1. Add a collison mesh to the planning scene: floor, desk, etc.
for cm in scene_collision_meshes:
scene.add_collision_mesh(cm)
if not LOAD_FROM_EXISTING:
scene.remove_collision_mesh('assembly')
# 2. Compute picking trajectory
picking_trajectory = plan_picking_motion(robot, picking_frame,
safelevel_picking_frame,
picking_configuration,
attached_element_mesh)
# 3. Save the last configuration from that trajectory as new
# start_configuration
start_configuration = Configuration(picking_trajectory.points[-1].values,
picking_trajectory.points[-1].types)
sequence = [key for key in assembly.network.nodes()]
sequence = list(range(10))
print(sequence)
exclude_keys = [
vkey for vkey in assembly.network.nodes_where({'is_planned': True})
]
sequence = [k for k in sequence if k not in exclude_keys]
print(sequence)
for key in sequence:
print("=" * 30 + "\nCalculating path for element with key %d." % key)
element = assembly.element(key)