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.joint_types == [Joint.REVOLUTE] * 6
config = Configuration.from_revolute_values([math.pi / 2, 0., 0.])
assert [math.degrees(value) for value in config.joint_values] == [90, 0, 0]
def trj():
p1 = JointTrajectoryPoint([1.571, 0, 0, 0.262, 0, 0], [0] * 6, [3.] * 6,
time_from_start=Duration(2, 1293))
p2 = JointTrajectoryPoint([0.571, 0, 0, 0.262, 0, 0], [0] * 6, [3.] * 6,
time_from_start=Duration(6, 0))
config = Configuration.from_revolute_values([0.] * 6)
return JointTrajectory(trajectory_points=[p1, p2],
joint_names=[
'joint_1', 'joint_2', 'joint_3', 'joint_4',
'joint_5', 'joint_6'
],
start_configuration=config)
import math
from compas.geometry import Frame
from compas.robots import Configuration
from compas_fab.backends import RosClient
with RosClient() as client:
robot = client.load_robot()
assert robot.name == 'ur5'
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.])
group = robot.main_group_name
# 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='RRTstarkConfigDefault'))
print("Computed kinematic path with %d configurations." %
len(trajectory.points))
print(
"Executing this path at full speed would take approx. %.3f seconds." %
from compas_fab.robots import CollisionMesh, AttachedCollisionMesh
with RosClient() as client:
robot = client.load_robot()
assert robot.name == 'ur5'
ee_link_name = robot.get_end_effector_link_name()
mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl'))
cm = CollisionMesh(mesh, 'tip')
acm = AttachedCollisionMesh(cm, link_name=ee_link_name)
client.add_attached_collision_mesh(acm)
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])
options = {
'max_step': 0.01,
'avoid_collisions': True,
}
trajectory = robot.plan_cartesian_motion(frames,
start_configuration,
options=options)
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)
print("Path plan computed with the attached collision meshes: {}".format(
[acm.collision_mesh.id for acm in trajectory.attached_collision_meshes]))
from compas.robots import Configuration
from compas_fab.robots.ur5 import Robot
robot = Robot()
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)
assert repr(frame_WCF.point) == 'Point(0.300, 0.100, 0.500)'
def joint_angles_to_configurations(robot, solutions, group=None):
joint_names = robot.get_configurable_joint_names(group=group)
return [
Configuration.from_revolute_values(q, joint_names=joint_names)
if q else None for q in solutions
]