from compas.geometry import Frame
from compas_fab.robots.ur5 import Robot
from compas_fab.backends 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)
from compas_fab.backends import RosClient
from compas_fab.robots import Configuration
from compas_fab.robots.ur5 import Robot
with RosClient() as client:
robot = Robot(client)
configuration = Configuration.from_revolute_values(
[-2.238, -1.153, -2.174, 0.185, 0.667, 0.])
frame_RCF = robot.forward_kinematics(configuration)
frame_WCF = robot.represent_frame_in_WCF(frame_RCF)
print("Frame in the robot's coordinate system")
print(frame_RCF)
print("Frame in the world coordinate system")
print(frame_WCF)
from compas_fab.backends import RosClient
from compas_fab.robots import Configuration
from compas_fab.robots.ur5 import Robot
with RosClient() as client:
robot = Robot(client)
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)
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
frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1])
tolerance_position = 0.001
tolerance_axes = [math.radians(1)] * 3
with RosClient() as client:
robot = Robot(client)
start_configuration = Configuration.from_revolute_values([-0.042, 4.295, 0, -3.327, 4.755, 0.])
group = robot.main_group_name
# create attached collision object
mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl'))
cm = CollisionMesh(mesh, 'tip')
ee_link_name = 'ee_link'
touch_links = ['wrist_3_link', 'ee_link']
acm = AttachedCollisionMesh(cm, ee_link_name, touch_links)
# create goal constraints from frame
goal_constraints = robot.constraints_from_frame(frame,
tolerance_position,
tolerance_axes,
"""Example: add a floor to the planning scene
"""
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('localhost') 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)