We've been discussing the dynamics of open-chain robots.

If the robot's motion is subject to constraints, however, like nonholonomic constraints due

to wheels or the loop-closure constraints of parallel robots, such as the Stewart platform,

we have to add forces that enforce the constraints.

Another example of a robot subject to constraints is a humanoid robot.

Here, the feet in contact with the ground create a closed loop, and if the robot grips

the box tightly, there is another closed loop through the arms.

These closed loops have to be enforced by constraint forces.

Finally, an open-chain robot erasing a whiteboard is another example of a robot subject to constraints.

We can write the constraints on the robot configuration as the vector equation b of

theta of t equals zero.

These configuration constraints keep the eraser pressed against the board.

Since these constraints are satisfied at all times, the time derivative of b must also

be zero.

By the chain rule, the time derivative can be expressed as the matrix of partial derivatives

of b with respect to theta times theta-dot, or A of theta times theta-dot for short, where

the A matrix is k-by-n, representing k velocity constraints on the n joints of the robot.

These Pfaffian velocity constraints can also represent nonholonomic constraints, as discussed

in Chapter 2.

We assume these k equality constraints are workless, meaning that the forces that enforce

these constraints do no work on the robot.

For the example of the robot erasing the board, this means that there is no friction between

the eraser and the board.

Without constraints, these are the equations of motion of the robot.

With constraints, the robot joint forces and torques tau may include forces against the

constraints, tau_con.

Thus the joint torques can be separated into components that move the robot and components

that act against the constraints.

Since the constraints are workless, the dot product of the torques against the constraints

with the joint velocities must be zero.

We also know that the velocity constraints have the form A theta-dot equals zero, so

therefore the constraint torques must be a linear combination of the rows of A, where

the k-vector of coefficients lambda is called a vector of Lagrange multipliers.

With this observation, we can rewrite our dynamics in this form.

Since the velocity constraints must be satisfied at all times, they can be expressed as constraints

on the acceleration.

These are now n-plus-k equations in n-plus-k variables, the k Lagrange multipliers and

either n joint accelerations or n joint torques, depending on whether we are solving the constrained

forward dynamics or the constrained inverse dynamics.

Skipping the derivation, which is given in the book, we can eliminate the k Lagrange

multipliers by defining an n-by-n projection matrix P of theta equal to the n-by-n identity

matrix minus A-transpose times the inverse of A M-inverse A-transpose times A M-inverse.

The rank of this n-by-n matrix is n-minus-k.

Using this projection matrix, we can define the constrained inverse dynamics, P times

tau equals P times M theta-double-dot plus h.

Since P is not invertible, we cannot premultiply both sides by P-inverse to get the unconstrained

inverse dynamics.

Instead, P projects the joint torques tau to the joint torques that move the robot,

eliminating the joint torques against the constraints that cause no motion of the robot.

To solve the inverse dynamics, we plug in the joint positions, velocities, and accelerations

on the right-hand side to calculate the joint torques that create the desired joint accelerations.

To this solution, we can add any joint torques of the form A-transpose lambda, which create

forces against the constraints and do not affect the motion of the robot.

This will be useful in Chapter 11 when we discuss hybrid motion-force control, where

we control the robot to achieve a desired motion in the unconstrained directions and

to achieve desired end-effector forces in the constrained directions.