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)))
def sphere_generator():
sphere = Sphere((0.35, 0, 0), 0.15)
for x in range(5):
for z in range(7):
center = sphere.point + Vector(x, 0, z) * 0.05
yield Sphere(center, sphere.radius)
# 2. Create 2D generator
def generator():
for sphere in sphere_generator():
yield points_on_sphere_generator(sphere)
# 3. Create reachability map 2D
with PyBulletClient(connection_type='direct') as client:
# load robot and define settings
robot = client.load_ur5(load_geometry=True)
ik = AnalyticalInverseKinematics(client)
client.inverse_kinematics = ik.inverse_kinematics
options = {"solver": "ur5", "check_collision": True, "keep_order": True}
# calculate reachability map
map = ReachabilityMap()
map.calculate(generator(), robot, options)
# save to json
map.to_json(os.path.join(DATA, "reachability", "map2D_spheres.json"))
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)