def testPostureConstraint(self):
r = pydrake.rbtree.RigidBodyTree(
os.path.join(pydrake.getDrakePath(),
"examples/Pendulum/Pendulum.urdf"))
q = -0.9
posture_constraint = ik.PostureConstraint(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(r)
results = ik.InverseKin(
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(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(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)
def runIkTraj(self, fields):
timeSamples = self.makeTimeSamplesFromConstraints(fields)
ikoptions = self.makeIkOptions(fields)
constraints = self.makeConstraints(fields)
q_nom = np.asarray(fields.poses[fields.nominalPose])
q_seed = np.asarray(fields.poses[fields.seedPose])
q_seed_end = np.asarray(fields.poses[fields.endPose])
q_nom_array = np.tile(q_nom, (len(timeSamples), 1)).transpose()
values = np.vstack((q_seed, q_seed_end))
timeRange = [timeSamples[0], timeSamples[-1]]
q_seed_array = self.getInterpolationFunction(
timeRange, values,
kind=self.trajInterpolationMode)(timeSamples).transpose()
assert q_seed_array.shape == (len(q_nom), len(timeSamples))
#print 'time range:', timeRange
#print 'time samples:', timeSamples
#print 'q_seed:', q_seed
#print 'q_seed_end:', q_seed_end
#print 'q_seed_array:', q_seed_array
#print 'q_nom_array:', q_nom_array
results = pydrakeik.InverseKinTraj(self.rigidBodyTree, timeSamples,
q_seed_array, q_nom_array,
constraints, ikoptions)
assert len(results.q_sol) == len(timeSamples)
poses = []
for i in xrange(len(results.q_sol)):
q = results.q_sol[i]
q.shape = q.shape[0]
poses.append(q)
info = results.info[0]
if fields.options.usePointwise:
pointwiseTimeSamples = np.linspace(timeRange[0], timeRange[1],
numPointwiseSamples)
q_seed_array = self.getInterpolationFunction(
timeSamples, np.array(poses), kind=self.trajInterpolationMode)(
pointwiseTimeSamples).transpose()
assert q_seed_array.shape == (len(q_nom), numPointwiseSamples)
results = pydrakeik.InverseKinPointwise(self.rigidBodyTree,
pointwiseTimeSamples,
q_seed_array, q_seed_array,
constraints, ikoptions)
assert len(results.q_sol) == len(pointwiseTimeSamples)
poses = []
for i in xrange(len(results.q_sol)):
q = results.q_sol[i]
q.shape = q.shape[0]
poses.append(q)
info = results.info[0]
timeSamples = pointwiseTimeSamples
assert timeSamples[0] == 0.0
# rescale timeSamples to respect max degrees per second option and min plan time
vel = np.diff(np.transpose(poses)) / np.diff(timeSamples)
timeScaleFactor = np.max(
np.abs(vel / np.radians(fields.options.maxDegreesPerSecond)))
timeScaleFactorMin = minPlanTime / timeSamples[-1]
timeScaleFactor = np.max([timeScaleFactor, timeScaleFactorMin])
timeSamples = np.array(timeSamples, dtype=float) * timeScaleFactor
# resample plan with warped time to adjust acceleration/deceleration profile
if useWarpTime:
tFine = np.linspace(timeSamples[0], timeSamples[-1], 100)
posesFine = self.getInterpolationFunction(timeSamples,
np.array(poses),
kind='linear')(tFine)
tFineWarped = warpTime(tFine / tFine[-1]) * tFine[-1]
timeSamples = np.linspace(tFineWarped[0], tFineWarped[-1],
numPointwiseSamples)
poses = self.getInterpolationFunction(
tFineWarped, posesFine,
kind=self.trajInterpolationMode)(timeSamples)
return poses, timeSamples, info
def runIkTraj(self, fields):
timeSamples = self.makeTimeSamplesFromConstraints(fields)
ikoptions = self.makeIkOptions(fields)
constraints = self.makeConstraints(fields)
q_nom = np.asarray(fields.poses[fields.nominalPose])
q_seed = np.asarray(fields.poses[fields.seedPose])
q_seed_end = np.asarray(fields.poses[fields.endPose])
q_nom_array = np.tile(q_nom, (len(timeSamples), 1)).transpose()
values = np.vstack((q_seed, q_seed_end))
timeRange = [timeSamples[0], timeSamples[-1]]
q_seed_array = self.getInterpolationFunction(
timeRange, values,
kind=self.trajInterpolationMode)(timeSamples).transpose()
assert q_seed_array.shape == (len(q_nom), len(timeSamples))
#print 'time range:', timeRange
#print 'time samples:', timeSamples
#print 'q_seed:', q_seed
#print 'q_seed_end:', q_seed_end
#print 'q_seed_array:', q_seed_array
#print 'q_nom_array:', q_nom_array
results = pydrakeik.InverseKinTraj(self.rigidBodyTree, timeSamples,
q_seed_array, q_nom_array,
constraints, ikoptions)
assert len(results.q_sol) == len(timeSamples)
poses = []
for i in xrange(len(results.q_sol)):
q = results.q_sol[i]
q.shape = q.shape[0]
poses.append(q)
info = results.info[0]
if fields.options.usePointwise:
numPointwiseSamples = 20
pointwiseTimeSamples = np.linspace(timeRange[0], timeRange[1],
numPointwiseSamples)
q_seed_array = self.getInterpolationFunction(
timeSamples, np.array(poses), kind=self.trajInterpolationMode)(
pointwiseTimeSamples).transpose()
assert q_seed_array.shape == (len(q_nom), numPointwiseSamples)
results = pydrakeik.InverseKinPointwise(self.rigidBodyTree,
pointwiseTimeSamples,
q_seed_array, q_seed_array,
constraints, ikoptions)
assert len(results.q_sol) == len(pointwiseTimeSamples)
print 'pointwise info len:', len(results.info)
poses = []
for i in xrange(len(results.q_sol)):
q = results.q_sol[i]
q.shape = q.shape[0]
poses.append(q)
info = results.info[0]
timeSamples = pointwiseTimeSamples
return poses, timeSamples, info
