Drake: Need Support for Mechanical Transmissions

[liangfok]

Does C++ Drake support mechanical transmissions where one joint is physically connected to another joint meaning they need to move together, though not necessarily in a 1:1 ratio?

If so, how is such a transmission specified and modeled in C++ Drake?

If not, what needs to change to support such mechanical coupling?

[sherm1]

You can do that with a very simple constraint q_i=r*q_j where r is the ratio; Drake could certainly do that. Is that what you meant or were you thinking of something higher level?

[amcastro-tri]

Yes, that's correct. It is done here for loop constraints. However for a simple constraint between two generalized velocities the infrastructure is not there

[liangfok]

Yes! I think a q_i = r * q_j constraint would be sufficient to inform the controller about the joint-to-joint transmission. How do we express such a constraint using Drake's Dynamics 1.0 API?

Also, I believe such a constraint would result in effort commands that make q_i move proportionally to q_j. Is this correct? In reality, the controller would not actually actuate q_j. Do we simply ignore the controller's effort command for q_j and implement, for example, a stiff PID controller for q_j whose reference position / velocity is that of q_i?

[amcastro-tri]

Right now we only have RigidBodyLoop which unfortunately does not inherit from anything else. Ideally you would like to have an abstract class Constraint and then you'd have something like TransmissionConstraint inheriting from constraint.
In order to define the constraint completely in Drake this implementation needs to provide the constraint error (phi in the link above), the constraint Jacobian and the Jacobian's time derivative times v (Jdotv in the link above).
Right now the current implementation in Drake uses a "full" Jacobian meaning that there will be a bunch of zeros whenever a dof does not participate in the constraint (instead of using the kinematic path between the two bodies). Also it uses a wacky version of Baumgarte stabilization, which is not good for variable time step integrators!

[hongkai-dai]

@liangfok , sorry I do not follow exactly. I thought that constraint should be imposed on both constraint force and actuator command, and the joint will be moved by both actuator command and constraint force.

My understanding is that to enforce q_i = r * q_j, we will define phi(q) = q_i-r*q_j, and introduce a linear constraint phi(q)=0, with its Jacobian as J. Then we want to say phi_ddot + 2*a*phi_dot+a^2*phi=0 to damp phi. This imposes a constraint on phi_ddot, which depends on constraint force and actuator command. Since q_j is not actuated, the actuator command u_j is further constraint to be equal to 0. We then solve the optimization problem with both constraints on phi_ddot and u_j, to find the actuator commands and constraint force together. The joint q_j will be moved due to the constraint force.

Do I misunderstand your question?

[amcastro-tri]

The first part is exactly right @hongkai-dai. However you do not need a constraint to make u_j=0, you will only actuate one of the joints, say q_i and then constraint q_j to follow. This is like having a bike chain and you only actuate on the pedals, not both the pedal and the cassette.

[hongkai-dai]

@amcastro-tri , I guess we are talking about the same thing. I was thinking that the decision variables for actuator commands include u_j, but in fact since it is un-actuated, I need to further impose that u_j=0. I guess what you are saying is that since there is no actuator for joint j, then u_j does not appear in the decision variable, and the acceleration q_ddot_j for joint j is caused by the constraint force only. If I am understanding you correctly, your approach to exclude u_j from decision variables is cleaner compared to imposing u_j=0.

[liangfok]

Two quick questions:

Question 1

phi(q) = q_i-r*q_j

Should phi(q) above be phi(q_i, q_j) since there is no independent variable called q?

Question 2

How is the following equation derived? What is variable a?

phi_ddot + 2*a*phi_dot + a^2*phi=0

[hongkai-dai]

To question 1:
Yes phi(q) should be phi(q_i,q_j).

To question 2
That constraint means that phi will exponentially converge to zero, a controls the rate of the convergence. A larger a means faster convergence to zero. The solution to

phi_ddot+2*a*phi_dot+a^2*phi=0

is

phi(t) = (c1+c2*t)*e^{-a*t}

where c1,c2 are 2D vectors, and depend on the initial value of phi at time 0

[liangfok]

Rough notes from F2F conversation with @amcastro-tri:

A general (that only depends on positions) constraint is fully determined by:

• The error function (or the constraint itself, phi): https://github.com/RobotLocomotion/drake/blob/master/drake/systems/plants/RigidBodyTree.cpp#L1857
• The jacobian (obtained by taking the derivative with respect to time of phi): https://github.com/RobotLocomotion/drake/blob/master/drake/systems/plants/RigidBodyTree.cpp#L1882
• The remaining acceleration terms obtained by taking the second derivative (usually Jdot*v): https://github.com/RobotLocomotion/drake/blob/master/drake/systems/plants/RigidBodyTree.cpp#L1901
  ^^^ Generalize to support more than just loop constraints.

In general the equation for stabilizing phi, as @hongkai-dai noticed, is that of a damped second order oscillator phi_ddot+2*zeta*a*phi_dot+a^2*phi=0 with a the natural frequency of oscillation when there is no damping and zeta a dimensionless damping factor. It turns out that with this dimensionless form the solution to this system is critically damped when zeta=1.0. That is the solution that asymptotically reaches zero the fastest.

The frequency is determined as a trade off. We want it to go to infinity (a to infinity) but that would make the system unstable. A small frequency would be visible in our solutions (spurious oscillation of frequency a). We then choose the biggest a that keeps our system stable.

[hongkai-dai]

@liangfok

phi_ddot+2*a*phi_dot+a^2*phi=0

is a linear homogeneous ODE on phi. So we can compute its solution. For more information, you could refer to this page https://en.wikipedia.org/wiki/Linear_differential_equation#Second-order_case. The reason why we want to impose this equation is to make phi converge exponentially to zero.

[sherm1]

What @hongkai-dai is describing is called Baumgarte stabilization. It is _not_ a good way to prevent constraint drift! The actual acceleration constraint is just phi_ddot=0. The Baumgarte terms are a hack to avoid drift off the velocity and position constraint manifolds. The weakest link in Baumgarte stabilization is the choice of a but there are many other problems with it. I prefer to use constraint projection for stabilization.

[sherm1]

Also regarding question 1, the conventional way to write these is phi(q)=0. In this case phi(q)=q_i-r*q_j but q_i,q_j are elements of q so IMO it is correct as written.

[amcastro-tri]

Yes, the Baumgarte stabilization it's a pretty hacky way to implement constraints (though I used it in the past :-/). A neat trick is to make a=alpha/dt with dt the time step so that now the parameter alpha is dimensionless and a lot more problem independent than the a=5.0 in Drake!!!

[liangfok]

The URDF specification for joint transmissions is the <mimic> tag, which is described here: http://wiki.ros.org/urdf/XML/joint. There is a discussion about Gazebo's support for this <mimic> tag here.

SDF does not appear to have such an abstraction at this point in time. There's an ongoing discussion here, which I linked to from here.

[sherm1]

BTW, there is a better way to specify constraints that does not require writing the Jacobian. In general constraints should not be reduced to apply forces directly on the generalized coordinates (joint axes) unless that is close the the actual physical mechanism being used. If forces are actually applied elsewhere, then the reaction forces in the joints and constraints will be wrong, even if the accelerations are right. I would like to discuss this at a whiteboard!

[hongkai-dai]

@sherm1 , I am working on this constraint. I would love to ask you with a whiteboard tomorrow.

[sherm1]

Please do!

[amcastro-tri]

@sherm1: that's such a good observation! what I was proposing would actually apply a torque to get the right acceleration! (and apply a non-zero work I think?) Please keep me up to date on how @hongkai-dai will implement this, I'm very curious on what the solution is.

[sherm1]

@amcastro-tri: your constraint would still have zero work. With some imagination you could come up with a physical mechanism that could work that way. Maybe hollow, geared shafts with other shafts rotating inside? Anyway, that mechanism would be workless since any power applied to one joint sucks power out of the other joint.

[amcastro-tri]

Then I guess I didn't understand your comment above:

BTW, there is a better way to specify constraints that does not require writing the Jacobian. In general constraints should not be reduced to apply forces directly on the generalized coordinates (joint axes)

But I can always wait till tomorrow or until @hongkai-dai PR's this.

[sherm1]

I was attempting to make two unrelated points:

• Applying generalized forces (joint torques in this case) is still a physically reasonable thing to do, but it corresponds to a particular physical mechanism that can generate pure torques (like a motor built into the joint shaft). If the actual mechanism applies off-axis forces it might still generate the same net torque, but it will also produce reaction forces ("squeezing" forces) that we need to model if we are interested in those effects.
• There is an easier way for a human to describe a constraint than to make said human write down an explicit Jacobian matrix, which is both difficult and computationally inefficient. The basic idea is to write four constant-time methods for each type of constraint:

c++ perr(context; q); // return position constraint error verr(context; v); // return d/dt perr aerr(context; vdot); // return d/dt verr force(context; lambda); // apply forces produced by constraint multiplier lambda

Inside, each of those is very simple because they use the multibody system's services to get pose, velocity, and acceleration information. There are also a lot of other benefits to this approach including the ability to multiply a vector by the constraint Jacobian or Jacobian transpose in O(n) time.

We would need to add an API for this to Drake. (I discussed this at the whiteboard with @hongkai-dai this morning -- much easier there!)

[hongkai-dai]

@amcastro-tri , I think the "Jacobian free" version will not appear in the incoming PR. It requires writing the multi-body system in an appropriate way, that we need to cache the spatial velocity and acceleration of each link when we call doKinematics function. But it would definitely save a lot of computation without computing the Jacobian in the future.

[amcastro-tri]

I figured. Thanks for the update!
@sherm1 will have to explain to me the "Jacobian free" version in our Thursdays VC.

[liangfok]

@hongkai-dai: Just wondering, was this was closed because it was addressed by a recently merged PR?

[amcastro-tri]

I will reopen this since we still do not have support for transmissions. The plan is that once the RBT templatization is finalized (#3987) I will implement a general framework for constraints that will allow us, among other things, to add transmissions.
The templatization work needs to be over to avoid having to work around template methods or CRTP in RBT.
I will therefore re-open this.

[hongkai-dai]

@liangfok I closed it because I think in system 2.0, we will have a more general solution, to handle the general case of position/velocity constraint, instead of hard-coding a transmission constraint as I did in system 1.0

[liangfok]

I see. I suppose even if System 2.0 supports a more general solution, API sugar should be provided to allow one to add mechanical transmissions.

[tri-ltyyu]

This is likely specific to RigidBodyPlant. If this is applicable to MultBodyPlant/SceneGraph, please file new issue. Closing for now.