Robot Manipulator Control

Position control, velocity control, force control, and WTF is impedance?

Introduction

For a given desired pose and orientation in task space (cartesian space) of the end-effector, how to control the robot arm?

Position Control

Poses and vels are provided from generated (task space) trajectory and we want to follow that.

1. For time coming time t, get the pose
2. Get the joint angles (and joint vels) using inverse kinematics (and diff ik)
3. Use the resulting joint angles as the desired angles in a PID controller which outputs motor torques based on current joint angle.
4. Repeat until end of trajectory

Based on the difference between desired angle and current angles (and the gains), the torques can be quite high and dangerous to humans.

Can use nested loops instead of simple one loop of feeding in pose and outputting power. Have an inner loop which accepts a target velocity and outputs power which runs more frequently. Makes the motion more smoother and accurate and stable.

Why position control?

Much more common then Torque Control For small robots, with large gear reductions, the number of dyn effects are very difficult to model (backlash, vibrations, and friction). - Simple relations between current and torque breaks down. - For Gear rations » 10, the unmodelled terms are so large, that torque cannot be simply related to current linearly. - Still, a monotonically increasing (tho not linear) relation between torque and current. - Transmission can only decrease energy from system not add. - So with increasing current, torque will increase (not linearly) and so will acceleration.

• Hence, torque control is not an option and position control is used instead.
  • Pose sensors (encoder or potenmeter) are cheap and accurate enough after filtering.
  • Enough for tracking desired pose traj of the arm using PID.

Velocity Control

If trajectory is given in velocities then, same logic as above but replace with joint velocities instead of joint angles.

Problem of ‘I’ in PID and why PD

• Time to react to gravity (droop) and why predetermined grav comp from used instead.
• Not safe (Integral windup) if against a wall; I term will keep increasing (windup) and generate absurd amount of forces.
• Vibrates due to Coriolis terms

Joint Trajectory Controller (in ROS)

• Feedforward (vel ff to overcome friction of motor; acc ff to overcome robot intertia at start)+ Feedback terms (to correct small errors) to generate power
• Feedforward term reduces lag and is more active so much more snappier to lock on traj target
• PD by itself will always lag behind the target as it needs to see the error (cannot predict).
• Feedforward handles bulk of movement proactively, feedback corrects model mismatches. \(u(t) = \underbrace{K_p e + K_d \dot{e} + K_i \int e}_{\text{The PID Feedback (Reactive)}} + \underbrace{K_{ffv} \dot{q}_d + K_{ffa} \ddot{q}_d}_{\text{The Feed-Forward (Proactive)}}\)

Grav comp term in control command

\(u = -\tau_g + K_p(q_d - q) + K_d(\dot q_d - \dot q)\)

• Removes steady state errors in PD (acts as the I term)
• Add to each joint

Inverse Dynamics feedforward terms

• Add acceleration feedforward term and mass term to cancel out ma terms.
• $u = -\tau_g + m[\ddot q_d + K_p(q_d - q) + K_d(\dot q_d - \dot q)]$
• Add to each joint
• Final dynamics eqn in task space: $\ddot{\bar z} + k_d \dot{\bar z} + k_p \bar z = 0$
• Errors dynamics converge nicely to zero
• Very good tracking performance

Torque-controlled

• Different then force control
• Can be applied when:
  • If gear reductions are small < 10: 1 and frictional forces are negligible.
  • MIT Cheetah, Barrett WAM, Boston dynamics
  • Have large gear ratios but add sensors to directly measure the torque at joint side of the actuator.
    • Used by KUKA iiwa
    • Strain gauges in the transmisson
  • Series-elastic actuators: Sawyer
    • Need quite low stiffness transmissions (5000Nm/rad to 100 Nm.rad)

(Direct) Force Control

• Generate desired contact forces:
• 3 Cases:
  • Acceleration not zero
  • Acceleration zero
  • Actual contact force can be measured at location (and hence regulated)
• Acceleration zero assumption
  • valid for many cases
  • in contact with object
  • control force : $u = -mg - f_{des}^{Fc}$
  • if not in contact will accelerate the robot to be in contact
  • no information about the geometry of the box (just basic pereception to position finger)

Indirect Force Control: Impedance Control

• Instead of specifying a desired contact force to apply, make the robot act as a mech system that reacts to contact forces in a desired way.
• Way of thinking called stiffness or impedance control
• Make the robot act like a spring-mass-damper system
  • Applies force back when it’s applied to it.
• Instead of rigidly following position setpoint commands, we can move the set points (and stiffness) of a virtual spring and allow the spring to generate our desired contact forces.
• Advantages:
  • Robot wont’ go unstable when faced by unmodelled contact interactions.
• Robot acts like a dissipative system
  • Env also dissipative
  • Entire system is guranteed to be stable
  • Guranteed for complex system
• Passivity theory or more generally Port-Hamiltonian systems
• Orign system of rqns
  • $m \begin{bmatrix} \ddot{x} \ \ddot{z} \end{bmatrix} = mg + u + f^{F_c}.$
• Stiffness controller (replace desired force with virtual mass-spring-damper), so automatically generate desired forced based on the parameters of the virtual mass-spring-damper system. Kinda-like a PD controller with additional feedforward term to cancel out gravity.
  • $m \begin{bmatrix} \ddot{x} \ \ddot{z} \end{bmatrix} + b \begin{bmatrix} \dot{x} \ \dot{z} \end{bmatrix} + k \begin{bmatrix} x - x_d \ z - z_d \end{bmatrix} = f^{F_c},$
  • $x_d, z_d$ desired waypoints
  • Can change the effective mass parameter of the stiffness controller as well to virtually change the “felt mass”, but not worth the complexity of extra sensing and bandwith.
• Usually, teleop is done with stiffness/impedance control!!!
• Stiffness controller in standard form: \(\)

Hybrid position/force control

• Applying a desired forces in some directions, and following pose trajectory in other.
• Polishing task example (apply constant desired force normal to surface, while moving tangentially in some pre-defined trajectory.)
• To achieve this simply apply a feed-forward force command (in directio u want) to the stiffness/impedace controller

\[m \begin{bmatrix} \ddot{x} \\ \ddot{z} \end{bmatrix} + b \begin{bmatrix} \dot{x} \\ \dot{z} \end{bmatrix} + k \begin{bmatrix} x - x_d \\ z - z_d \end{bmatrix} = f^{F_c},\]

General case (using manipulator eqns)

• Prev sections assumed point floating finger/gripper
• Apply to entire robot manipulator
• Equations of dynamics for the manipulator (w/o) contact: \(M(q)\ddot q + C(q, \dot q) \dot q = \tau_g (q) + u\)
• Forward Dynamics Problem: Calculate accelerations $\ddot q$ given the rest (used for sim)
• Inverse Dynamics Problem: Calculate $u$ given the rest (used for control) \(u = M(q)\ddot q_d + C(q, \dot q)\dot q - \tau_g(q)\)
  • Given a $\ddot q_d$ finding $u$
  • In practice have estimate of the dynamic parameters (mass, etc) and pose and vel too.
  • Need to account for these errors when we determine acc commands to send.
• Ex: follow a joint trajectory $q_0(t)$
  • We do not just differentiate twice and input $\ddot q_0(t)$
  • Instead: $\ddot q_d = \ddot q_0 (t) + K_p(q_0(t) - q) + K_d (\dot q_0 (t) - \dot q)$
  • To account for th pose and vel errors (along with dynamics error)
  • Called “inverse dynamics control” or “feedforward-plus-feedback-linearizing control

Joint Stiffness control

• Impedance controller in configuration (joint) space: \(u = -\tau_g(q) + K_p (q_d - q) + K_d(\dot q_d - \dot q) - \tau_{ff}\)
• $K_p, K_d$ are pos diag matrices for all the joints, $\tau_ff$ is the “feedforward” torque (optional, not needed for positional control needed for force control).

Cartesian stiffness and operational space control

• Writing manipulator dyn in ee frame \(M_E(q)\ddot{p}^E + C_E(q, \dot{q})\dot{q} = f_g^{B_E}(q) + f_u^{B_E} + f_{\text{ext}}^{B_E},\)
• Where: \(M_E(q) = (JM^{-1}J^T)^{-1}, \quad C_E(q, \dot{q}) = M_E(JM^{-1}C - \dot{J}), \quad f_g^{B_E}(q) = M_E JM^{-1}\tau_g.\)
• So u (force) is simply analogous to the joint space control:
• \[u = -mg + K_p(p^{F_d} - p^F) + K_d(v^{F_d} - v^F) - f_{\text{feedforward}}^F.\]
• If have redundant joints (7dof)
  • Can do “dynamically-consistent null-space projection” where you have the stiffness controller iwth multiple objectives with lower priority objective in the nulls-ace of the primary task Look more into this.

Difference b/w Position (PD) control and Stiffness control

• Difference is quite small
• Stiffness control tries to cancel out gravity and any other estimated terms (like friction).
  • We have this estimation models for some robot arms but not form many with large gear-ratio transmissions
  • Nor do we have torque control
• But this small difference makes a big impact.
  • the controller no longer depends on error to achieve joint pose in steady state
  • Hence, can turn the gain wayyy down, and have more compliant response, achieving good tracking when there are no external torques.
• Stiffness/Impedance control is extra if you have parameters of the manipulator eqns
  • Different philosophy of thinking about the external forces
  • Treat them as MDS for derivation.
  • In pose control, we don’t have a model, just plug and play a PD controller.