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_nom_array = np.tile(q_nom, (len(timeSamples), 1)).transpose() # todo # q_seed_array should be interpolated from startPose (seedPose) to endPose q_seed_array = np.tile(q_seed, (len(timeSamples), 1)).transpose() results = pydrakeik.InverseKinTraj(self.rigidBodyTree, timeSamples, q_seed, q_nom, constraints, ikoptions) 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] return poses, info
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 plan_grasping_trajectory(rbt, q0, target_reach_pose, target_grasp_pose, n_knots, reach_time, grasp_time): ''' Solves IK at a series of sample times (connected with a cubic spline) to generate a trajectory to bring the Kuka from an initial pose q0 to a final end effector pose in the specified time, using the specified number of knot points. Uses an intermediate pose reach_pose as an intermediate target to hit at the knot point closest to reach_time. See http://drake.mit.edu/doxygen_cxx/rigid__body__ik_8h.html for the "inverseKinTraj" entry. At the moment, the Python binding for this function uses "inverseKinTrajSimple" -- i.e., it doesn't return derivatives. ''' nq = rbt.get_num_positions() q_des_full = np.zeros(nq) # Create knot points ts = np.linspace(0., grasp_time, n_knots) # Figure out the knot just before reach time reach_start_index = np.argmax(ts >= reach_time) - 1 controlled_joint_names = [ "iiwa_joint_1", "iiwa_joint_2", "iiwa_joint_3", "iiwa_joint_4", "iiwa_joint_5", "iiwa_joint_6", "iiwa_joint_7" ] free_config_inds, constrained_config_inds = \ kuka_utils.extract_position_indices(rbt, controlled_joint_names) # Assemble IK constraints constraints = [] # Constrain the non-searched-over joints for all time all_tspan = np.array([0., grasp_time]) posture_constraint = ik.PostureConstraint(rbt, all_tspan) posture_constraint.setJointLimits(constrained_config_inds, q0[constrained_config_inds] - 0.01, q0[constrained_config_inds] + 0.01) constraints.append(posture_constraint) # Constrain all joints to be the initial posture at the start time start_tspan = np.array([0., 0.]) posture_constraint = ik.PostureConstraint(rbt, start_tspan) posture_constraint.setJointLimits(free_config_inds, q0[free_config_inds] - 0.01, q0[free_config_inds] + 0.01) constraints.append(posture_constraint) # Constrain the ee frame to lie on the target point # facing in the target orientation in between the # reach and final times ee_frame = rbt.findFrame("iiwa_frame_ee").get_frame_index() for i in range(reach_start_index, n_knots): this_tspan = np.array([ts[i], ts[i]]) interp = float(i - reach_start_index) / (n_knots - reach_start_index) target_pose = (1. - interp) * target_reach_pose + interp * target_grasp_pose constraints.append( ik.WorldPositionConstraint(rbt, ee_frame, np.zeros((3, 1)), target_pose[0:3] - 0.01, target_pose[0:3] + 0.01, tspan=this_tspan)) constraints.append( ik.WorldEulerConstraint(rbt, ee_frame, target_pose[3:6] - 0.05, target_pose[3:6] + 0.05, tspan=this_tspan)) # Seed and nom are both the initial repeated for the # # of knot points q_seed = np.tile(q0, [1, n_knots]) q_nom = np.tile(q0, [1, n_knots]) options = ik.IKoptions(rbt) # Set bounds on initial and final velocities zero_velocity = np.zeros(rbt.get_num_velocities()) options.setqd0(zero_velocity, zero_velocity) options.setqdf(zero_velocity, zero_velocity) results = ik.InverseKinTraj(rbt, ts, q_seed, q_nom, constraints, options) qtraj = PiecewisePolynomial.Pchip(ts, np.vstack(results.q_sol).T, True) print "IK returned a solution with info %d" % results.info[0] print "(Info 1 is good, other values are dangerous)" return qtraj, results.info[0]
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 trajectoryIK(self, start, end): dur = self.config["trajectory_duration"] joints = np.array( [self.positions[joint] for joint in self.joint_names], dtype=np.int32 ).reshape(9, 1) constraints = list() # Constrain the start position start_pose = np.concatenate(start).reshape(9, 1) start_constraint = ik.PostureConstraint( self.robot, np.array([0.0, 0.0]).reshape([2,1]) # Active time ) start_constraint.setJointLimits( joints, start_pose - self.config['pose_tol'], start_pose + self.config['pose_tol'] ) constraints.append(start_constraint) # Constrain the end position end_pose = np.concatenate(end).reshape(9, 1) end_constraint = ik.PostureConstraint( self.robot, np.array([dur, dur]).reshape([2,1]) # Active time ) end_constraint.setJointLimits( joints, end_pose - self.config['pose_tol'], end_pose + self.config['pose_tol'] ) constraints.append(end_constraint) # Constrain against collisions constraints.append( ik.MinDistanceConstraint ( self.robot, self.config['collision_min_distance'], # Minimum distance between bodies list(), # Active bodies (empty set means all bodies) set() # Active collision groups (not filter groups! Empty set means all) )) # Prepare times of interest for trajectory solve times = np.linspace(0, dur, num=self.config['trajectory_count'], endpoint=True) # Compute cubic interpolation as seed trajectory x = np.array([0, dur]) y = np.hstack([start_pose, end_pose]) f = CubicSpline(x, y, axis=1, bc_type='clamped') q_seed = f(times) # print "q_seed:", q_seed # Solve the IK problem options = ik.IKoptions(self.robot) options.setMajorIterationsLimit(400) options.setFixInitialState(True) # print "Iterations limit:", options.getMajorIterationsLimit() result = ik.InverseKinTraj(self.robot, times, q_seed, q_seed, constraints, options) if result.info[0] != 1: print "Could not find safe trajectory!" print "Start pose:", start_pose.flatten() print "End pose: ", end_pose.flatten() print "Result:", result.info return None else: # Clean up: Pack solutions as a matrix, with time as first column q_sol = np.vstack(result.q_sol) q_sol = np.insert(q_sol, [0], times[:, None], axis=1) return q_sol
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
def plan_trajectory_to_posture(rbt, q0, qf, n_knots, duration, start_time=0.): ''' Solves IK at a series of sample times (connected with a cubic spline) to generate a trajectory to bring the Kuka from an initial pose q0 to a final end effector pose in the specified time, using the specified number of knot points. Uses an intermediate pose reach_pose as an intermediate target to hit at the knot point closest to reach_time. See http://drake.mit.edu/doxygen_cxx/rigid__body__ik_8h.html for the "inverseKinTraj" entry. At the moment, the Python binding for this function uses "inverseKinTrajSimple" -- i.e., it doesn't return derivatives. ''' nq = rbt.get_num_positions() # Create knot points ts = np.linspace(0., duration, n_knots) controlled_joint_names = [ "iiwa_joint_1", "iiwa_joint_2", "iiwa_joint_3", "iiwa_joint_4", "iiwa_joint_5", "iiwa_joint_6", "iiwa_joint_7" ] free_config_inds, constrained_config_inds = \ kuka_utils.extract_position_indices(rbt, controlled_joint_names) # Assemble IK constraints constraints = [] q0 = np.clip(q0, rbt.joint_limit_min, rbt.joint_limit_max) qf = np.clip(qf, rbt.joint_limit_min[free_config_inds], rbt.joint_limit_max[free_config_inds]) #constraints.append(ik.MinDistanceConstraint( # model=rbt, min_distance=0.01, active_bodies_idx=list(), # active_group_names=set())) # Constrain the non-searched-over joints for all time all_tspan = np.array([0., duration]) posture_constraint = ik.PostureConstraint(rbt, all_tspan) posture_constraint.setJointLimits(constrained_config_inds, q0[constrained_config_inds] - 0.01, q0[constrained_config_inds] + 0.01) constraints.append(posture_constraint) # Constrain all joints to be the initial posture at the start time # TODO: actually freeze these, rather than letting them be searched over. # No point actually searching over these, and makes IK way slower start_tspan = np.array([0., 0.]) posture_constraint = ik.PostureConstraint(rbt, start_tspan) posture_constraint.setJointLimits(free_config_inds, q0[free_config_inds] - 0.01, q0[free_config_inds] + 0.01) constraints.append(posture_constraint) # Constrain constrainted joints to be the final posture at the final time end_tspan = np.array([duration, duration]) posture_constraint = ik.PostureConstraint(rbt, end_tspan) posture_constraint.setJointLimits(free_config_inds, qf[free_config_inds] - 0.01, qf[free_config_inds] + 0.01) constraints.append(posture_constraint) # Seed is joint-space linear interpolation of the # initial and final posture (a reasonable initial guess) q_seed = np.zeros([q0.shape[0], n_knots]) qf_full = q0 * 0. qf_full[free_config_inds] = qf for i, t in enumerate(ts): frac = t / duration q_seed[:, i] = q0 * (1. - frac) + qf_full * (frac) # Nominal is the final posture q_nom = np.tile(qf_full, [1, n_knots]) options = ik.IKoptions(rbt) # Set bounds on initial and final velocities zero_velocity = np.zeros(rbt.get_num_velocities()) options.setqd0(zero_velocity, zero_velocity) options.setqdf(zero_velocity, zero_velocity) results = ik.InverseKinTraj(rbt, ts, q_seed, q_nom, constraints, options) ts += start_time qtraj = PiecewisePolynomial.Pchip(ts, np.vstack(results.q_sol).T, True) print "IK returned a solution with info %d" % results.info[0] return qtraj, results.info[0]
def plan_ee_trajectory(rbt, q0, traj_seed, ee_times, ee_poses, ee_body, ee_point, n_knots, start_time=0., avoid_collision_tspans=[], active_bodies_idx=list()): nq = rbt.get_num_positions() # Create knot points ts = np.linspace(0., ee_times[-1], n_knots) # Insert one more for each ee time if not already in there for t in ee_times: if t not in ts: ts = np.insert(ts, np.argmax(ts >= t), t) print "times ", ts, " for times ", ee_times controlled_joint_names = [ "iiwa_joint_1", "iiwa_joint_2", "iiwa_joint_3", "iiwa_joint_4", "iiwa_joint_5", "iiwa_joint_6", "iiwa_joint_7" ] free_config_inds, constrained_config_inds = \ kuka_utils.extract_position_indices(rbt, controlled_joint_names) # Assemble IK constraints constraints = [] for tspan in avoid_collision_tspans: constraints.append( ik.MinDistanceConstraint(model=rbt, min_distance=1E-6, active_bodies_idx=active_bodies_idx, active_group_names=set(), tspan=tspan)) print "active for ", tspan # Make starting constraint sensible if it's out of joint limits q0 = np.clip(q0, rbt.joint_limit_min, rbt.joint_limit_max) # Constrain the non-searched-over joints for all time all_tspan = np.array([0., ts[-1]]) posture_constraint = ik.PostureConstraint(rbt, all_tspan) posture_constraint.setJointLimits(constrained_config_inds, q0[constrained_config_inds] - 0.05, q0[constrained_config_inds] + 0.05) constraints.append(posture_constraint) # Constrain all joints to be the initial posture at the start time start_tspan = np.array([0., 0.]) posture_constraint = ik.PostureConstraint(rbt, start_tspan) posture_constraint.setJointLimits(free_config_inds, q0[free_config_inds] - 0.01, q0[free_config_inds] + 0.01) constraints.append(posture_constraint) # Constrain the ee frame to lie on the target point # facing in the target orientation in between the # reach and final times) for t, pose in zip(ee_times, ee_poses): this_tspan = np.array([t - 0.01, t + 0.01]) constraints.append( ik.WorldPositionConstraint(rbt, ee_body, ee_point, pose[0:3] - 0.01, pose[0:3] + 0.01, tspan=this_tspan)) constraints.append( ik.WorldEulerConstraint(rbt, ee_body, pose[3:6] - 0.05, pose[3:6] + 0.05, tspan=this_tspan)) # Seed and nom are both the initial repeated for the # # of knot points q_seed = np.hstack([traj_seed.value(t) for t in ts]) q_nom = np.tile(q0, [ts.shape[0], 1]).T options = ik.IKoptions(rbt) # Set bounds on initial and final velocities zero_velocity = np.zeros(rbt.get_num_velocities()) options.setqd0(zero_velocity, zero_velocity) options.setqdf(zero_velocity, zero_velocity) Q = np.eye(q0.shape[0], q0.shape[0]) Q[free_config_inds[-2:], free_config_inds[-2:]] *= 1 options.setQ(Q) options.setQv(np.eye(q0.shape[0], q0.shape[0]) * 10) results = ik.InverseKinTraj(rbt, ts, q_nom, q_seed, constraints, options) qtraj = PiecewisePolynomial.Pchip(ts + start_time, np.vstack(results.q_sol).T, True) print "IK returned a solution with info %d" % results.info[0] return qtraj, results.info[0], results.q_sol