예제 #1
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_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
예제 #2
0
    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)
예제 #3
0
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]
예제 #4
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
예제 #6
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:
            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
예제 #7
0
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]
예제 #8
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