CODEWITHSUNDEEP
drake
matheecs
matheecs

Reputation: 43

How to implement custom constraint using C++?

I am trying to reimplement littledog.ipynb using C++. I find it is hard to translate the function velocity_dynamics_constraint and have 3 questions

  1. What is the function of ad_velocity_dynamics_context? Can we ignore it?
  2. How to reimplement velocity_dynamics_constraint using C++? Do I have to create a new class like class VelocityDynamicsConstraint : public drake::solvers::Constraint? Is three any easier way to implement it?
  3. Why we need to consider isinstance(vars[0], AutoDiffXd) condition?
# Some code from https://github.com/RussTedrake/underactuated/blob/master/examples/littledog.ipynb
ad_velocity_dynamics_context = [
    ad_plant.CreateDefaultContext() for i in range(N)
]

def velocity_dynamics_constraint(vars, context_index):
    h, q, v, qn = np.split(vars, [1, 1+nq, 1+nq+nv])
    if isinstance(vars[0], AutoDiffXd):
        if not autoDiffArrayEqual(
                q,
                ad_plant.GetPositions(
                    ad_velocity_dynamics_context[context_index])):
            ad_plant.SetPositions(
                ad_velocity_dynamics_context[context_index], q)
        v_from_qdot = ad_plant.MapQDotToVelocity(
            ad_velocity_dynamics_context[context_index], (qn - q) / h)
    else:
        if not np.array_equal(q, plant.GetPositions(
                context[context_index])):
            plant.SetPositions(context[context_index], q)
        v_from_qdot = plant.MapQDotToVelocity(context[context_index],
                                              (qn - q) / h)
    return v - v_from_qdot
for n in range(N-1):
    prog.AddConstraint(partial(velocity_dynamics_constraint,
                                context_index=n),
                        lb=[0] * nv,
                        ub=[0] * nv,
                        vars=np.concatenate(
                            ([h[n]], q[:, n], v[:, n], q[:, n + 1]))

Upvotes: 2

Views: 159

Answers (1)

Hongkai Dai
Hongkai Dai

Reputation: 2766

What is the function of ad_velocity_dynamics_context? Can we ignore it?

The context caches the intermediate computation result for a given q, v, u. It is very common that several constraints are imposed on the same set of q, v, u (for example, consider at the final time, we typically have a kinematic constraint on the final state, say the robot foot has to land on the ground and its center of mass is at a certain location. At the same time we we have the velocity dynamics constraint on the final state). Hence these different constraints can share some intermediate computation result, such as the rigid transform between each adjacent links. Hence we cache the result in ad_velocity_dynamics_context, and this ad_velocity_dynamics_context can be used later when we impose other constraints.

How to reimplement velocity_dynamics_constraint using C++? Do I have to create a new class like class VelocityDynamicsConstraint : public drake::solvers::Constraint? Is there any easier way to implement it?

That is right, you will need to create a new class VelocityDynamicsConstraint. The main challenge in implementing this class is to write the three overloaded DoEval function for three scalar types (double, AutoDiffXd, symbolic::Expression). You can refer to PositionConstraint as a reference. And for the moment you can ignore the case to call DoEval(const Eigen::Ref<const AutoDiffXd>&, AutoDiffXd*) with a MultibodyPlant<double> case, and only implement the this DoEval function with MultibodyPlant<AutoDiffXd>.

Why we need to consider isinstance(vars[0], AutoDiffXd) condition?

Because when the scalar type is AutoDiffXd, we want to compare not only the value of q against the one stored in context, but also its gradient. If they are different, then we need to call SetPositions to recompute the cache. When the scalar type is double, we then only need to compare the value.

Upvotes: 2

Related Questions