Symbol Definitions

Mass matrix in Joint Space

Centrifugal and Coriolis Forces term in Joint Space

Orientation transformation matrix

Force applied to end effector in Task Space

Force applied to end effector for unit mass in Task Space

Gravitational force in Joint Space

Jacobian matrix

Linear Velocity Jacobian

Angular Velocity Jacobian

Proportional gain

Velocity gain (damping gain)

Gravitational force in Task Space

Joint positions

Desired rotation matrix of end effector

Rotation matrix column

Linear velocity of end effector

End effector position in Cartesian coordinates

Mass matrix in Task Space

Centrifugal and Coriolis Forces term in Task Space

Generalized Joint Torques

Angular velocity of end effector

Operators
#

Inverse Jacobian for Kinetic Energy minimization

Psuedo Inverse

Cross product operator

For all other symbols, means this is the theoretical estimate of the value based on kinematic
equations.

Control Formulation
Dynamics Equation & Jacobian
First, we formulate the dynamics equation in joint space and task space:
Joint Space
+ + =

(1)

+ + =

(2)

Task Space
We use the basic Jacobian to translate from Joint Space to Task Space. The Jacobian is defined as
follows:
=

(3a)

[ ] = [ ]

(3b)

We translate from task space forces and the joint space generalized forces through the following
relationships:
=

(4a)

= #

(4b)

Where we define the inverse Jacobian using kinetic energy minimization:

# = 1 (1 )1

(5)

Control Law
Next, we formulate our control law. We estimate the mass matrix, centrifugal forces, Coriolis forces,
and gravity using the mechanical properties of the Kuka IIWA.
Control Force
= + +

(6)

Where the mass matrix estimate is defined as

= 1

(7)

Next, we define F as the force required for a unit mass (identity mass matrix).
= + ( ) + ( )

(8)

We split up the position control and orientation control of our control law to simplify the software
implementation.

Position Control
For Position Control, E+ is the identity matrix.
= 33
We do not have desired velocity, so our desired velocity is zero. Also, we ignore the centrifugal and
Coriolis forces since they can create instabilities in implementation. From Equation 6, we calculate our
control force in Task Space.
= [ ( ) ] +
Lastly, from Equation 4b, we have the joint torques we directly send to the IIWA,
= # { [ ( ) ] + }

(9)

Orientation Control
For Orientation Control, we use the Direction Cosines formulation. See Professor Khatibs Advanced
Robotic Manipulation Course Notes for full derivation. In this formulation, E+ takes the form
1
= [2 ]
3 93
From Equation 8, Orientation error can also be rewritten as
1
2

+ ( ) = (1 1 + 2 2 + 3 3 )

(10)

Where our desired rotation orientation is defined as

= [1

3 ]33

We substitute Equation 10 into Equation 8. We ignore the desired velocity term:

=
We ignore gravity & Coriolis and centrifugal forces because of instabilities. The angular velocity is found
by applying Equation 3b. From Equation 6 we find the control F:
1
= [ (1 1 + 2 2 + 3 3 )]
2
Lastly we use Equation 4b to find the joint torques:
1

#
=
{ [ 2 (1 1 + 2 2 + 3 3 )] }

(11)

In software, we use the Multitask Controller in SCL to combine the joint torques in Equations 9 and 11.
We send this joint torque to the robot to execute our Control Formulation.