def connect_to_ros(request, doctest_namespace):
if request.module.__name__ == 'compas_fab.robots.robot':
doctest_namespace["robot"] = Robot()
yield
elif request.module.__name__ == 'compas_fab.robots.planning_scene':
with RosClient() as client:
robot = Robot(client)
doctest_namespace["client"] = client
doctest_namespace["robot"] = robot
yield
else:
yield
def connect_to_ros(doctest_namespace):
client = RosClient()
client.run()
robot = Robot(client)
doctest_namespace["client"] = client
doctest_namespace["robot"] = robot
yield
client.close()
def visualize_urscript(script):
M = [
[-1000, 0, 0, 0],
[0, 1000, 0, 0],
[0, 0, 1000, 0],
[0, 0, 0, 1],
]
rgT = matrix_to_rgtransform(M)
cgT = Transformation.from_matrix(M)
robot = Robot()
viz_planes = []
movel_matcher = re.compile(r"^\s*move([lj]).+((-?\d+\.\d+,?\s?){6}).*$")
for line in script.splitlines():
mo = re.search(movel_matcher, line)
if mo:
if mo.group(1) == "l": # movel
ptX, ptY, ptZ, rX, rY, rZ = mo.group(2).split(",")
pt = Point(float(ptX), float(ptY), float(ptZ))
pt.transform(cgT)
frame = Frame(pt, [1, 0, 0], [0, 1, 0])
R = Rotation.from_axis_angle_vector(
[float(rX), float(rY), float(rZ)], pt)
T = matrix_to_rgtransform(R)
plane = cgframe_to_rgplane(frame)
plane.Transform(T)
viz_planes.append(plane)
else: # movej
joint_values = mo.group(2).split(",")
configuration = Configuration.from_revolute_values(
[float(d) for d in joint_values])
frame = robot.forward_kinematics(configuration)
plane = cgframe_to_rgplane(frame)
plane.Transform(rgT)
viz_planes.append(plane)
return viz_planes
def test_inverse_kinematics2():
ik = AnalyticalInverseKinematics(
solver="ur5") # set the solver in the beginning
robot = Robot()
frame_WCF = Frame((0.381, 0.093, 0.382), (0.371, -0.292, -0.882),
(0.113, 0.956, -0.269))
# set the solver later
solutions = list(
ik.inverse_kinematics(robot,
frame_WCF,
start_configuration=None,
group=None))
assert (len(solutions) == 8)
joint_positions, _ = solutions[0]
correct = Configuration.from_revolute_values(
(0.022, 4.827, 1.508, 1.126, 1.876, 3.163))
assert (correct.close_to(
Configuration.from_revolute_values(joint_positions)))
import time
from compas.datastructures import Mesh
import compas_fab
from compas_fab.backends import RosClient
from compas_fab.robots import CollisionMesh
from compas_fab.robots import PlanningScene
from compas_fab.robots.ur5 import Robot
with RosClient() as client:
robot = Robot(client)
scene = PlanningScene(robot)
mesh = Mesh.from_stl(compas_fab.get('planning_scene/floor.stl'))
cm = CollisionMesh(mesh, 'floor')
scene.add_collision_mesh(cm)
# sleep a bit before terminating the client
time.sleep(1)
from compas.geometry import Frame
from compas_fab.robots.ur5 import Robot
from compas_fab.backends.kinematics import AnalyticalInverseKinematics
ik = AnalyticalInverseKinematics()
robot = Robot()
frame_WCF = Frame((0.381, 0.093, 0.382), (0.371, -0.292, -0.882),
(0.113, 0.956, -0.269))
for jp, jn in ik.inverse_kinematics(
robot, frame_WCF,
options={'solver':
'ur5'}): # knows that we need the IK for the UR5 robot
print(jp)
