def test_backward(self, robot: Robot, pose: _pose.Pose):
# kinematics object
ik = Kinematics()
# solve the inverse kinematics
res_backward = ik.backward(robot, pose)
nl, _ = robot.num_dimensionality
kc: kinematicchain.KinematicChain
assert res_backward.pose == pose
assert res_backward.lengths.ndim == 1
assert res_backward.lengths.shape == (robot.num_kinematic_chains, )
assert (0 <= res_backward.lengths).all()
assert res_backward.directions.shape == (robot.num_kinematic_chains,
nl)
for idxkc, kc in enumerate(robot.kinematic_chains):
fanchor = robot.frame.anchors[kc.frame_anchor].linear.position
platform = robot.platforms[kc.platform]
panchor = platform.anchors[kc.platform_anchor].linear.position
assert res_backward.leave_points[idxkc, 0:nl] \
== pytest.approx(fanchor[0:nl])
assert np.linalg.norm(res_backward.directions[idxkc, 0:nl]) \
== pytest.approx(1)
assert pose.linear.position[0:nl] \
+ pose.angular.dcm.dot(panchor)[0:nl] \
+ res_backward.directions[idxkc, 0:nl] \
* res_backward.lengths[idxkc] \
== pytest.approx(fanchor[0:nl])
def test_forward(self, robot: Robot, pose: _pose.Pose):
# kinematics object
ik = Kinematics()
# first, calculate cable lengths to run forward kinematics
res_backward = ik.backward(robot, pose)
# now, solve forward kinematics
res_forward = ik.forward(robot, res_backward.lengths)
nl, _ = robot.num_dimensionality
kc: kinematicchain.KinematicChain
assert res_forward.pose.linear.position \
== pytest.approx(res_backward.pose.linear.position,
abs=1e-4, rel=1e-4)
assert res_forward.pose.angular.quaternion \
== pytest.approx(res_backward.pose.angular.quaternion,
abs=1e-4, rel=1e-4)
assert res_forward.lengths.ndim == 1
assert res_forward.lengths.shape == (robot.num_kinematic_chains, )
assert (0 <= res_forward.lengths).all()
assert res_forward.directions.shape == (robot.num_kinematic_chains, nl)
for idxkc, kc in enumerate(robot.kinematic_chains):
fanchor = robot.frame.anchors[kc.frame_anchor].linear.position
platform = robot.platforms[kc.platform]
panchor = platform.anchors[kc.platform_anchor].linear.position
assert res_backward.leave_points[idxkc, 0:nl] \
== pytest.approx(fanchor[0:nl])
assert np.linalg.norm(res_backward.directions[idxkc, 0:nl]) \
== pytest.approx(1)
def test_render_kinematics_1t(self, engine: _engine.Engine,
ik_standard: Standard, zero_pose: pose.Pose,
robot_1t: _robot.Robot):
robot = robot_1t
pose = zero_pose
kinematics = ik_standard.backward(robot, pose)
wizard = visualization.Visualizer(engine)
wizard.render(robot)
wizard.render(kinematics)
wizard.draw()
wizard.engine._figure.write_image(f'{sane_string(robot.name).lower()}-'
f'home'
f'.pdf')
wizard.close()
def test_render_kinematics_1t_random(self, engine: _engine.Engine,
ik_standard: Standard,
rand_pose_1t: pose.Pose,
robot_1t: _robot.Robot):
robot = robot_1t
pose = rand_pose_1t
kinematics = ik_standard.backward(robot, pose)
robot.platforms[0].pose = pose
wizard = visualization.Visualizer(engine)
wizard.render(robot)
wizard.render(kinematics)
wizard.draw()
wizard.engine._figure.write_image(f'{sane_string(robot.name).lower()}-'
f'random'
f'.pdf')
wizard.close()
def test_render_kinematics_ipanema3(self, engine: _engine.Engine,
ik_pulley: Standard,
zero_pose: pose.Pose,
ipanema_3: _robot.Robot):
robot = ipanema_3
pose = zero_pose
kinematics = ik_pulley.backward(robot, pose)
robot.platforms[0].pose = pose
wizard = visualization.Visualizer(engine)
wizard.render(robot)
wizard.render(kinematics)
wizard.draw()
wizard.engine._figure.write_image(f'{sane_string(robot.name).lower()}-'
f'home'
f'.pdf')
wizard.close()
class VisualizationPlotlyTestSuite(object):
@pytest.mark.parametrize(
('engine', 'geometry'),
itertools.product((
plotly.Linear(),
plotly.Planar(),
plotly.Spatial(),
), (
geometry.Cuboid(1.00, 2.00, 3.0),
geometry.Cylinder(1.00, 2.00),
geometry.Cylinder([1.00, 2.00], 3.00),
geometry.Ellipsoid(1.00),
geometry.Ellipsoid([1.00, 2.00, 3.00]),
geometry.Tube(1.00, 2.00, 3.00),
geometry.Tube([1.00, 2.00], 3.00, 4.00),
geometry.Tube([1.00, 2.00], [3.00, 4.00], 5.00),
)),
ids=('{}-{}'.format(*v) for v in itertools.product((
'linear',
'planar',
'spatial',
), (
'cuboid',
'cylinder',
'cylinder-elliptical',
'ellipsoid',
'ellipsoid-elliptical',
'tube',
'2-tubes',
'2-elliptical-tubes',
))),
)
def test_render_geometry(self, engine: _engine.Engine,
geometry: Primitive):
wizard = visualization.Visualizer(engine)
wizard.render(geometry)
wizard.draw()
wizard.engine._figure.write_image(
f'{sane_string(geometry.__name__.lower())}'
f'.pdf')
wizard.close()
@pytest.mark.parametrize(('engine', ), [
(plotly.Linear(), ),
])
def test_render_kinematics_1t(self, engine: _engine.Engine,
ik_standard: Standard, zero_pose: pose.Pose,
robot_1t: _robot.Robot):
robot = robot_1t
pose = zero_pose
kinematics = ik_standard.backward(robot, pose)
wizard = visualization.Visualizer(engine)
wizard.render(robot)
wizard.render(kinematics)
wizard.draw()
wizard.engine._figure.write_image(f'{sane_string(robot.name).lower()}-'
f'home'
f'.pdf')
wizard.close()
@pytest.mark.parametrize(('engine', ), [
(plotly.Linear(), ),
])
def test_render_kinematics_1t_random(self, engine: _engine.Engine,
ik_standard: Standard,
rand_pose_1t: pose.Pose,
robot_1t: _robot.Robot):
robot = robot_1t
pose = rand_pose_1t
kinematics = ik_standard.backward(robot, pose)
robot.platforms[0].pose = pose
wizard = visualization.Visualizer(engine)
wizard.render(robot)
wizard.render(kinematics)
wizard.draw()
wizard.engine._figure.write_image(f'{sane_string(robot.name).lower()}-'
f'random'
f'.pdf')
wizard.close()
@pytest.mark.parametrize(('engine', ), [
(plotly.Planar(), ),
])
def test_render_kinematics_2t(self, engine: _engine.Engine,
ik_standard: Standard, zero_pose: pose.Pose,
robot_2t: _robot.Robot):
robot = robot_2t
pose = zero_pose
kinematics = ik_standard.backward(robot, pose)
robot.platforms[0].pose = pose
wizard = visualization.Visualizer(engine)
wizard.render(robot)
wizard.render(kinematics)
wizard.draw()
wizard.engine._figure.write_image(f'{sane_string(robot.name).lower()}-'
f'home'
f'.pdf')
wizard.close()
@pytest.mark.parametrize(('engine', ), [
(plotly.Planar(), ),
])
def test_render_kinematics_2t_random(self, engine: _engine.Engine,
ik_standard: Standard,
rand_pose_2t: pose.Pose,
robot_2t: _robot.Robot):
robot = robot_2t
pose = rand_pose_2t
kinematics = ik_standard.backward(robot, pose)
robot.platforms[0].pose = pose
wizard = visualization.Visualizer(engine)
wizard.render(robot)
wizard.render(kinematics)
wizard.draw()
wizard.engine._figure.write_image(f'{sane_string(robot.name).lower()}-'
f'random'
f'.pdf')
wizard.close()
@pytest.mark.parametrize(('engine', ), [
(plotly.Spatial(), ),
])
def test_render_kinematics_3t(self, engine: _engine.Engine,
ik_standard: Standard, zero_pose: pose.Pose,
robot_3t: _robot.Robot):
robot = robot_3t
pose = zero_pose
kinematics = ik_standard.backward(robot, pose)
robot.platforms[0].pose = pose
wizard = visualization.Visualizer(engine)
wizard.render(robot)
wizard.render(kinematics)
wizard.draw()
wizard.engine._figure.write_image(f'{sane_string(robot.name).lower()}-'
f'home'
f'.pdf')
wizard.close()
@pytest.mark.parametrize(('engine', ), [
(plotly.Spatial(), ),
])
def test_render_kinematics_3t_random(self, engine: _engine.Engine,
ik_standard: Standard,
rand_pose_3t: pose.Pose,
robot_3t: _robot.Robot):
robot = robot_3t
pose = rand_pose_3t
kinematics = ik_standard.backward(robot, pose)
robot.platforms[0].pose = pose
wizard = visualization.Visualizer(engine)
wizard.render(robot)
wizard.render(kinematics)
wizard.draw()
wizard.engine._figure.write_image(f'{sane_string(robot.name).lower()}-'
f'random'
f'.pdf')
wizard.close()
@pytest.mark.parametrize(('engine', ), [
(plotly.Planar(), ),
])
def test_render_kinematics_1r2t(self, engine: _engine.Engine,
ik_standard: Standard,
zero_pose: pose.Pose,
robot_1r2t: _robot.Robot):
robot = robot_1r2t
pose = zero_pose
kinematics = ik_standard.backward(robot, pose)
robot.platforms[0].pose = pose
wizard = visualization.Visualizer(engine)
wizard.render(robot)
wizard.render(kinematics)
wizard.draw()
wizard.engine._figure.write_image(f'{sane_string(robot.name).lower()}-'
f'home'
f'.pdf')
wizard.close()
@pytest.mark.parametrize(('engine', ), [
(plotly.Planar(), ),
])
def test_render_kinematics_1r2t_random(self, engine: _engine.Engine,
ik_standard: Standard,
rand_pose_1r2t: pose.Pose,
robot_1r2t: _robot.Robot):
robot = robot_1r2t
pose = rand_pose_1r2t
kinematics = ik_standard.backward(robot, pose)
robot.platforms[0].pose = pose
wizard = visualization.Visualizer(engine)
wizard.render(robot)
wizard.render(kinematics)
wizard.draw()
wizard.engine._figure.write_image(f'{sane_string(robot.name).lower()}-'
f'random'
f'.pdf')
wizard.close()
@pytest.mark.parametrize(('engine', ), [
(plotly.Spatial(), ),
])
def test_render_kinematics_2r3t(self, engine: _engine.Engine,
ik_standard: Standard,
zero_pose: pose.Pose,
robot_2r3t: _robot.Robot):
robot = robot_2r3t
pose = zero_pose
kinematics = ik_standard.backward(robot, pose)
robot.platforms[0].pose = pose
wizard = visualization.Visualizer(engine)
wizard.render(robot)
wizard.render(kinematics)
wizard.draw()
wizard.engine._figure.write_image(f'{sane_string(robot.name).lower()}-'
f'home'
f'.pdf')
wizard.close()
@pytest.mark.parametrize(('engine', ), [
(plotly.Spatial(), ),
])
def test_render_kinematics_2r3t_random(self, engine: _engine.Engine,
ik_standard: Standard,
rand_pose_2r3t: pose.Pose,
robot_2r3t: _robot.Robot):
robot = robot_2r3t
pose = rand_pose_2r3t
kinematics = ik_standard.backward(robot, pose)
robot.platforms[0].pose = pose
wizard = visualization.Visualizer(engine)
wizard.render(robot)
wizard.render(kinematics)
wizard.draw()
wizard.engine._figure.write_image(f'{sane_string(robot.name).lower()}-'
f'random'
f'.pdf')
wizard.close()
@pytest.mark.parametrize(('engine', ), [
(plotly.Spatial(), ),
])
def test_render_kinematics_3r3t(self, engine: _engine.Engine,
ik_standard: Standard,
zero_pose: pose.Pose,
robot_3r3t: _robot.Robot):
robot = robot_3r3t
pose = zero_pose
kinematics = ik_standard.backward(robot, pose)
robot.platforms[0].pose = pose
wizard = visualization.Visualizer(engine)
wizard.render(robot)
wizard.render(kinematics)
wizard.draw()
wizard.engine._figure.write_image(f'{sane_string(robot.name).lower()}-'
f'home'
f'.pdf')
wizard.close()
@pytest.mark.parametrize(('engine', ), [
(plotly.Spatial(), ),
])
def test_render_kinematics_3r3t_random(self, engine: _engine.Engine,
ik_standard: Standard,
rand_pose_3r3t: pose.Pose,
robot_3r3t: _robot.Robot):
robot = robot_3r3t
pose = rand_pose_3r3t
kinematics = ik_standard.backward(robot, pose)
robot.platforms[0].pose = pose
wizard = visualization.Visualizer(engine)
wizard.render(robot)
wizard.render(kinematics)
wizard.draw()
wizard.engine._figure.write_image(f'{sane_string(robot.name).lower()}-'
f'random'
f'.pdf')
wizard.close()
@pytest.mark.parametrize(('engine', ), [
(plotly.Spatial(), ),
])
def test_render_kinematics_ipanema3(self, engine: _engine.Engine,
ik_pulley: Standard,
zero_pose: pose.Pose,
ipanema_3: _robot.Robot):
robot = ipanema_3
pose = zero_pose
kinematics = ik_pulley.backward(robot, pose)
robot.platforms[0].pose = pose
wizard = visualization.Visualizer(engine)
wizard.render(robot)
wizard.render(kinematics)
wizard.draw()
wizard.engine._figure.write_image(f'{sane_string(robot.name).lower()}-'
f'home'
f'.pdf')
wizard.close()
@pytest.mark.parametrize(('engine', ), [
(plotly.Spatial(), ),
])
def test_render_kinematics_ipanema3_random(self, engine: _engine.Engine,
ik_pulley: Standard,
rand_pose_3r3t: pose.Pose,
ipanema_3: _robot.Robot):
robot = ipanema_3
pose = rand_pose_3r3t
kinematics = ik_pulley.backward(robot, pose)
robot.platforms[0].pose = pose
wizard = visualization.Visualizer(engine)
wizard.render(robot)
wizard.render(kinematics)
wizard.draw()
wizard.engine._figure.write_image(f'{sane_string(robot.name).lower()}-'
f'random'
f'.pdf')
wizard.close()
@pytest.mark.parametrize(
('wizard', 'robot', 'algorithm'),
((visualization.Visualizer(plotly.Linear()), rob,
workspace.grid.Algorithm(
archetype.Translation(dcm=np.eye(3)),
criterion.CableLength(Standard(),
np.asarray([0.5, 1.5]) * np.sqrt(1)),
[-1.0], [1.0], 9)) for rob in ([robots.robot_1t()])))
def test_render_linear_workspace(self, wizard: visualization.Visualizer,
robot: _robot.Robot,
algorithm: _workspace.Algorithm):
# evaluate the workspace
ws = algorithm.evaluate(robot)
# visualize the result
wizard.render(robot)
wizard.render(ws)
wizard.draw()
wizard.engine._figure.write_image(
f'{sane_string(robot.name).lower()}-'
f'{sane_string(algorithm.criterion.name).lower()}'
f'.pdf')
wizard.close()
@pytest.mark.parametrize(
('wizard', 'robot', 'algorithm'),
((visualization.Visualizer(plotly.Planar()), rob,
workspace.grid.Algorithm(
archetype.Translation(dcm=np.eye(3)),
criterion.CableLength(Standard(),
np.asarray([0.5, 1.5]) * np.sqrt(2)),
[-1.0, -1.0], [1.0, 1.0], 9))
for rob in (robots.robot_2t(), robots.robot_1r2t())))
def test_render_planar_workspace(self, wizard: visualization.Visualizer,
robot: _robot.Robot,
algorithm: _workspace.Algorithm):
# evaluate the workspace
ws = algorithm.evaluate(robot)
# visualize the result
wizard.render(robot)
wizard.render(ws)
wizard.draw()
wizard.engine._figure.write_image(
f'{sane_string(robot.name).lower()}-'
f'{sane_string(algorithm.criterion.name).lower()}'
f'.pdf')
wizard.close()
@pytest.mark.parametrize(
('wizard', 'robot', 'algorithm'), (itertools.chain(
((visualization.Visualizer(plotly.Spatial()), rob,
workspace.grid.Algorithm(
archetype.Translation(dcm=np.eye(3)),
criterion.CableLength(Standard(),
np.asarray([0.5, 1.5]) * np.sqrt(3)),
[-1.0, -1.0, -1.0], [1.0, 1.0, 1.0], 9))
for rob in (robots.robot_3t(), robots.robot_2r3t(),
robots.robot_3r3t())),
((visualization.Visualizer(plotly.Spatial()), rob,
workspace.hull.Algorithm(
archetype.Translation(dcm=np.eye(3)),
criterion.CableLength(Standard(),
np.asarray([0.5, 1.5]) * np.sqrt(3)),
[0.0, 0.0, 0.0], 9))
for rob in (robots.robot_3t(), robots.robot_2r3t(),
robots.robot_3r3t())))))
def test_render_spatial_workspace(self, wizard: visualization.Visualizer,
robot: _robot.Robot,
algorithm: _workspace.Algorithm):
# evaluate the workspace
ws = algorithm.evaluate(robot)
# visualize the result
wizard.render(robot)
wizard.render(ws)
wizard.draw()
wizard.engine._figure.write_image(
f'{sane_string(robot.name).lower()}-'
f'{sane_string(algorithm.criterion.name).lower()}'
f'.pdf')
wizard.close()