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