def test_revolute_ctor():
q = [4.5, 1.7, 0.5, 2.1, 0.1, 2.1]
config = Configuration.from_revolute_values(q)
assert config.types == [Joint.REVOLUTE] * 6
config = Configuration.from_revolute_values([pi / 2, 0., 0.])
assert to_degrees(config.values) == [90, 0, 0]
def compas_fab_ik_fn(frame_rcf, sampled=[]):
tool_pose = pose_from_frame(frame_rcf)
conf_vals = compute_inverse_kinematics(ikfast_ik_fn,
tool_pose,
sampled=sampled)
return [
Configuration.from_revolute_values(q) for q in conf_vals
if q is not None and len(q) > 0
]
def from_joint_trajectory(
cls, joint_trajectory
): # type: (JointTrajectory) -> MinimalTrajectory
"""Construct a instance from a :class:`compas_fab.robots.JointTrajectory`."""
conf_list = [
Configuration.from_revolute_values(pt.values)
for pt in joint_trajectory.points
]
return cls(conf_list)
def calculate_configurations_for_path(frames, robot, current_positions=[]):
"""Calculate possible configurations for a path.
Args:
frames (Frame): the path described with frames
Returns:
configurations: list of list of float
"""
configurations = []
for i, frame in enumerate(frames):
configs = robot.inverse_kinematics(frame)
qsols = [c.values for c in configs]
if not len(qsols):
return []
if i == 0:
if len(current_positions):
qsols_formatted = []
for jp_a in qsols:
jp_a_formatted = format_joint_positions(
jp_a, current_positions)
qsols_formatted.append(jp_a_formatted)
configurations.append(qsols_formatted)
else:
configurations.append(qsols)
else:
previous_qsols = configurations[-1][:]
qsols_sorted = []
for jp_b in previous_qsols:
diffs = []
qsols_formatted = []
for jp_a in qsols:
jp_a_formatted = format_joint_positions(jp_a, jp_b)
qsols_formatted.append(jp_a_formatted)
diffs.append(
sum([
math.fabs(qa - qb)
for qa, qb in zip(jp_a_formatted, jp_b)
]))
selected_idx = diffs.index(min(diffs))
qsols_sorted.append(qsols_formatted[selected_idx])
configurations.append(qsols_sorted)
print(len(configurations))
print(len(configurations[0]))
configurations = list(zip(*configurations))
print(len(configurations))
for i in range(len(configurations)):
configurations[i] = list(configurations[i])
for j, q in enumerate(configurations[i]):
configurations[i][j] = Configuration.from_revolute_values(q)
return configurations
def inverse_kinematics(self, tool0_frame_RCS):
"""Inverse kinematics function.
Args:
tool0_frame_RCS (:class:`Frame`): The tool0 frame to reach in robot
coordinate system (RCS).
Returns:
configurations (:obj:`list` of :class:`Configuration`): A list
of possible configurations.
"""
solutions = inverse_kinematics(tool0_frame_RCS, self.params)
configurations = []
for joint_values in solutions:
configurations.append(
Configuration.from_revolute_values(joint_values))
return configurations
def __init__(
self,
rob_conf: confuse.AttrDict,
pick_station: PickStation,
ros_port: int = 9090,
):
super().__init__(ros_port=ros_port)
self.rob_conf = rob_conf
self.pick_place_tool = rob_conf.tools.get("pick_place")
self.wobjs = rob_conf.wobjs
self.global_speed_accel = rob_conf.robot_movement.global_speed_accel
self.joint_positions = rob_conf.robot_movement.joint_positions
self.speed = rob_conf.robot_movement.speed
self.zone = rob_conf.robot_movement.zone
self.docker_cfg = rob_conf.docker
# Setup pick station
self.pick_station = pick_station
if hasattr(self.joint_positions, "travel_trajectory"):
joint_positions = [
to_radians(jp) for jp in self.joint_positions.travel_trajectory
]
configurations = [
Configuration.from_revolute_values(jps)
for jps in joint_positions
]
trajectory = MinimalTrajectory(configurations)
trajectories = MinimalTrajectories([trajectory])
self.default_travel_trajectories = trajectories
self.default_return_travel_trajectories = (
trajectories.reversed_recursively())
log.debug(
f"default_travel_trajectories: {self.default_travel_trajectories}"
)
from compas_fab.backends import RosClient
from compas_fab.robots import Configuration
with RosClient() as client:
robot = client.load_robot()
assert robot.name == 'ur5'
configuration = Configuration.from_revolute_values(
[-2.238, -1.153, -2.174, 0.185, 0.667, 0.])
frame_WCF = robot.forward_kinematics(configuration)
print("Frame in the world coordinate system")
print(frame_WCF)
import os
from compas.geometry import Frame
from compas.robots import LocalPackageMeshLoader
from compas.robots import RobotModel
from compas_fab.robots import Configuration
from ur_kinematics import inverse_kinematics_ur5
models_path = os.path.join(os.path.dirname(__file__), 'models')
loader = LocalPackageMeshLoader(models_path, 'ur_description')
model = RobotModel.from_urdf_file(loader.load_urdf('ur5.urdf'))
f = Frame((0.417, 0.191, -0.005), (-0.000, 1.000, 0.000),
(1.000, 0.000, 0.000))
f.point /= 0.001
sols = inverse_kinematics_ur5(f)
for joint_values in sols:
joint_names = model.get_configurable_joint_names()
print(Configuration.from_revolute_values(joint_values, joint_names))
def test_circle_cartesian(fixed_waam_setup, viewer, planner_ik_conf):
urdf_filename, semantics, robotA_tool = fixed_waam_setup
planner_id, ik_engine = planner_ik_conf
move_group = 'robotA'
print('\n')
print('=' * 10)
cprint(
'Cartesian planner {} with IK engine {}'.format(planner_id, ik_engine),
'yellow')
with PyChoreoClient(viewer=viewer) as client:
robot = client.load_robot(urdf_filename)
robot.semantics = semantics
robot_uid = client.get_robot_pybullet_uid(robot)
base_link_name = robot.get_base_link_name(group=move_group)
ik_joint_names = robot.get_configurable_joint_names(group=move_group)
tool_link_name = robot.get_end_effector_link_name(group=move_group)
base_frame = robot.get_base_frame(group=move_group)
if ik_engine == 'default-single':
ik_solver = None
elif ik_engine == 'lobster-analytical':
ik_solver = InverseKinematicsSolver(robot, move_group,
ik_abb_irb4600_40_255,
base_frame, robotA_tool.frame)
elif ik_engine == 'ikfast-analytical':
ikfast_fn = get_ik_fn_from_ikfast(ikfast_abb_irb4600_40_255.get_ik)
ik_solver = InverseKinematicsSolver(robot, move_group, ikfast_fn,
base_frame, robotA_tool.frame)
else:
raise ValueError('invalid ik engine name.')
init_conf = Configuration.from_revolute_values(np.zeros(6),
ik_joint_names)
# replace default ik function with a customized one
if ik_solver is not None:
client.planner.inverse_kinematics = ik_solver.inverse_kinematics_function(
)
tool_link = link_from_name(robot_uid, tool_link_name)
robot_base_link = link_from_name(robot_uid, base_link_name)
ik_joints = joints_from_names(robot_uid, ik_joint_names)
# * draw EE pose
tcp_pose = get_link_pose(robot_uid, tool_link)
draw_pose(tcp_pose)
# * generate multiple circles
circle_center = np.array([2, 0, 0.2])
circle_r = 0.2
# full_angle = np.pi
# full_angle = 2*2*np.pi
angle_range = (-0.5 * np.pi, 0.5 * np.pi)
# total num of path pts, one path point per 5 degree
ee_poses = compute_circle_path(circle_center, circle_r, angle_range)
ee_frames_WCF = [frame_from_pose(ee_pose) for ee_pose in ee_poses]
options = {'planner_id': planner_id}
if planner_id == 'LadderGraph':
client.set_robot_configuration(robot, init_conf)
st_time = time.time()
# options.update({'ik_function' : lambda pose: compute_inverse_kinematics(ikfast_abb_irb4600_40_255.get_ik, pose, sampled=[])})
trajectory = client.plan_cartesian_motion(robot,
ee_frames_WCF,
group=move_group,
options=options)
cprint(
'W/o frame variant solving time: {}'.format(
elapsed_time(st_time)), 'blue')
cprint(
'Cost: {}'.format(
compute_trajectory_cost(trajectory,
init_conf_val=init_conf.values)),
'blue')
print('-' * 5)
f_variant_options = {'delta_yaw': np.pi / 3, 'yaw_sample_size': 5}
options.update({
'frame_variant_generator':
PyChoreoFiniteEulerAngleVariantGenerator(
options=f_variant_options)
})
print('With frame variant config: {}'.format(f_variant_options))
client.set_robot_configuration(robot, init_conf)
st_time = time.time()
trajectory = client.plan_cartesian_motion(robot,
ee_frames_WCF,
group=move_group,
options=options)
cprint(
'{} solving time: {}'.format(
'With frame variant '
if planner_id == 'LadderGraph' else 'Direct',
elapsed_time(st_time)), 'cyan')
cprint(
'Cost: {}'.format(
compute_trajectory_cost(trajectory,
init_conf_val=init_conf.values)),
'cyan')
if trajectory is None:
cprint(
'Client Cartesian planner {} CANNOT find a plan!'.format(
planner_id), 'red')
else:
cprint(
'Client Cartesian planning {} find a plan!'.format(planner_id),
'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()
loader = RosFileServerLoader(client)
urdf = loader.load_urdf()
srdf = loader.load_srdf()
model = RobotModel.from_urdf_string(urdf)
semantics = RobotSemantics.from_srdf_string(srdf, model)
robot = Robot(model, semantics=semantics, client=client)
group = robot.main_group_name
frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1])
tolerance_position = 0.001
tolerance_axes = [math.radians(1)] * 3
start_configuration = Configuration.from_revolute_values(
(0.667, -0.298, 0.336, -2.333, -1.787, 2.123, 0.571))
# create goal constraints from frame
goal_constraints = robot.constraints_from_frame(frame, tolerance_position,
tolerance_axes, group)
trajectory = robot.plan_motion(goal_constraints,
start_configuration,
group,
planner_id='RRT')
print("Computed kinematic path with %d configurations." %
len(trajectory.points))
print(
"Executing this path at full speed would take approx. %.3f seconds." %
trajectory.time_from_start)
import math
from compas.geometry import Frame
from compas_fab.backends import RosClient
from compas_fab.robots import Configuration
with RosClient('localhost') as client:
robot = client.load_robot()
group = robot.main_group_name
frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1])
tolerance_position = 0.001
tolerance_axes = [math.radians(1)] * 3
start_configuration = Configuration.from_revolute_values(
[-0.042, 4.295, 0, -3.327, 4.755, 0.])
# create goal constraints from frame
goal_constraints = robot.constraints_from_frame(frame, tolerance_position,
tolerance_axes, group)
trajectory = robot.plan_motion(goal_constraints,
start_configuration,
group,
options=dict(planner_id='RRT'))
print("Computed kinematic path with %d configurations." %
len(trajectory.points))
print(
"Executing this path at full speed would take approx. %.3f seconds." %
trajectory.time_from_start)
"""
Calculate the inverse kinematics of a robot based on the frame and a starting
configuration.
"""
from ex23_load_robot import robot
from compas.geometry import Frame
from compas_fab.backends import RosClient
from compas_fab.robots import Configuration
robot.client = RosClient()
robot.client.run()
frame = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
start_configuration = Configuration.from_revolute_values([0] * 6)
group = "manipulator" # or robot.main_group_name
response = robot.inverse_kinematics(frame,
start_configuration,
group,
constraints=None,
attempts=50)
print(response.configuration)
robot.client.close()
robot.client.terminate()
from compas.geometry import Frame
from compas_fab.backends import RosClient
from compas_fab.robots import Configuration
with RosClient('localhost') as client:
robot = client.load_robot()
group = robot.main_group_name
frames = []
frames.append(Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]))
frames.append(Frame([0.5, 0.1, 0.6], [1, 0, 0], [0, 1, 0]))
start_configuration = Configuration.from_revolute_values([-0.042, 0.033, -2.174, 5.282, -1.528, 0.000])
trajectory = robot.plan_cartesian_motion(frames,
start_configuration,
group=group,
options=dict(
max_step=0.01,
avoid_collisions=True,
))
print("Computed cartesian path with %d configurations, " % len(trajectory.points))
print("following %d%% of requested trajectory." % (trajectory.fraction * 100))
print("Executing this path at full speed would take approx. %.3f seconds." % trajectory.time_from_start)
# - 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)
print(full_cfg)
def joint_angles_to_configurations(A1, A2, A3, A4, A5, A6):
return [
Configuration.from_revolute_values([a1, a2, a3, a4, a5, a6])
for a1, a2, a3, a4, a5, a6 in zip(A1, A2, A3, A4, A5, A6)
]
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 forward_kinematics(self, configuration):
q = configuration.values[:]
q[5] += math.pi
return super(UR3, self).forward_kinematics(
Configuration.from_revolute_values(q))
Configuration.from_revolute_values(q))
def inverse_kinematics(self, tool0_frame_RCS):
configurations = super(UR3, self).inverse_kinematics(tool0_frame_RCS)
for q in configurations:
print(q)
for i in range(len(configurations)):
configurations[i].values[5] -= math.pi
return configurations
if __name__ == "__main__":
import math
from compas_fab.utilities import sign
from compas.geometry import Frame
from .kinematics import format_joint_positions
ur = UR3()
q = [-0.44244, -1.5318, 1.34588, -1.38512, -1.05009, -0.4495]
q = Configuration.from_revolute_values(q)
Ts = ur.get_forward_transformations(q)
for T in Ts:
print(T)
print()
frame = ur.forward_kinematics(q)
qsols = ur.inverse_kinematics(frame)
for q in qsols:
print(q)
ur.get_transformed_model(Ts)
# create tool from json
filepath = os.path.join(DATA, "vacuum_gripper.json")
tool = Tool.from_json(filepath)
element_height = 0.014
safelevel_height = 0.05
with RosClient('localhost') as client:
robot = client.load_robot()
# 1. Set tool
robot.attach_tool(tool)
# 2. Define start configuration
start_configuration = Configuration.from_revolute_values(
[-5.961, 4.407, -2.265, 5.712, 1.571, -2.820])
# 3. Define frames
picking_frame = Frame([-0.43, 0, element_height], [1, 0, 0], [0, 1, 0])
safelevel_picking_frame = Frame(
[-0.43, 0, element_height + safelevel_height], [1, 0, 0], [0, 1, 0])
frames = [picking_frame, safelevel_picking_frame]
# 4. Convert frames to tool0_frames
frames_tool0 = robot.from_tcf_to_t0cf(frames)
trajectory = robot.plan_cartesian_motion(frames_tool0,
start_configuration,
options=dict(max_step=0.01))
print("Computed cartesian path with %d configurations, " %
from compas.geometry import Frame
from compas.datastructures import Mesh
from compas_fab.robots import Configuration
from compas_fab.backends import RosClient
HERE = os.path.dirname(__file__)
DATA = os.path.join(HERE, '../data')
PATH = os.path.join(DATA, 'cylinder.obj')
robot.client = RosClient()
robot.client.run()
goal_frame = Frame([0.20, 0.38, 0.32], [0, 1, 0], [0, 0, 1])
start_configuration = Configuration.from_revolute_values((-0.042, -1.988, 2.174, -3.327, -1.528, -6.283))
group = robot.main_group_name
# Create attached collision object
cylinder = Mesh.from_obj(PATH)
aco = robot.create_collision_mesh_attached_to_end_effector('cylinder', cylinder, group)
response = robot.motion_plan_goal_frame(goal_frame,
start_configuration,
tolerance_position=0.001,
tolerance_angle=math.radians(1),
group=group,
path_constraints=None,
planner_id='RRT',
num_planning_attempts=20,
allowed_planning_time=8.,
