def testPostureConstraint(self):
q = -0.9
posture_constraint = ik.PostureConstraint(self.r)
posture_constraint.setJointLimits(np.array([[6]], dtype=np.int32),
np.array([[q]]),
np.array([[q]]))
# Choose a seed configuration (randomly) and a nominal configuration
# (at 0)
q_seed = np.vstack((np.zeros((6, 1)),
0.8147))
q_nom = np.vstack((np.zeros((6, 1)),
0.))
options = ik.IKoptions(self.r)
results = ik.InverseKin(
self.r, q_seed, q_nom, [posture_constraint], options)
self.assertEqual(results.info[0], 1)
self.assertAlmostEqual(results.q_sol[0][6], q, 1e-9)
# Run the tests again both pointwise and as a trajectory to
# validate the interfaces.
t = np.array([0., 1.])
q_seed_array = np.transpose(np.array([q_seed, q_seed]))[0]
q_nom_array = np.transpose(np.array([q_nom, q_nom]))[0]
results = ik.InverseKinPointwise(self.r, t, q_seed_array, q_nom_array,
[posture_constraint], options)
self.assertEqual(len(results.info), 2)
self.assertEqual(len(results.q_sol), 2)
self.assertEqual(results.info[0], 1)
# The pointwise result will go directly to the constrained
# value.
self.assertAlmostEqual(results.q_sol[0][6], q, 1e-9)
self.assertEqual(results.info[1], 1)
self.assertAlmostEqual(results.q_sol[1][6], q, 1e-9)
results = ik.InverseKinTraj(self.r, t, q_seed_array, q_nom_array,
[posture_constraint], options)
self.assertEqual(len(results.info), 1)
self.assertEqual(len(results.q_sol), 2)
self.assertEqual(results.info[0], 1)
# The trajectory result starts at the initial value and moves
# to the constrained value.
self.assertAlmostEqual(results.q_sol[0][6], q_seed[6], 1e-9)
self.assertAlmostEqual(results.q_sol[1][6], q, 1e-9)
np.array([np.nan, np.nan, 0.0])),
ik.WorldPositionConstraint(robot, base_body_id, np.array([1.0, 0.0, 0.0]),
np.array([np.nan, np.nan, 0.0]),
np.array([np.nan, np.nan, 0.0])),
ik.WorldPositionConstraint(robot, base_body_id, np.array([0.0, 1.0, 0.0]),
np.array([np.nan, np.nan, 0.0]),
np.array([np.nan, np.nan, 0.0])),
# This constraint exactly specifies the desired position of the
# hand frame we defined earlier.
ik.WorldPositionConstraint(robot, hand_frame_id, np.array([0.0, 0.0, 0.0]),
np.array([0.5, 0.0, 0.6]),
np.array([0.5, 0.0, 0.6])),
# And this specifies the orientation of that frame
ik.WorldEulerConstraint(robot, hand_frame_id, np.array([0.0, 0.0, 0.0]),
np.array([0.0, 0.0, 0.0]))
]
q_seed = robot.getZeroConfiguration()
options = ik.IKoptions(robot)
results = ik.InverseKin(robot, q_seed, q_seed, constraints, options)
# Each entry (only one is present in this case, since InverseKin()
# only returns a single result) in results.info gives the output
# status of SNOPT. info = 1 is good, anything less than 10 is OK, and
# any info >= 10 indicates an infeasibility or failure of the
# optimizer.
assert results.info[0] == 1
print(repr(results.q_sol[0]))