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()