#### 8.7. Constrained Dynamics

This video describes the dynamics of robots when they are subject to constraints, such as loop-closure constraints or nonholonomic constraints. Lagrange multipliers, modeling forces against constraints, are introduced, as well as projection methods that eliminate explicit calculation of the Lagrange multipliers.

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.