[SOLVED] Cmpsci 403: introduction to robotics: perception, mechanics, dynamics, and control hw 07

1 Operational Space Control
In HW6, you made a dynamics simulation for the robot shown in Figure 1. Given a desired position of the
foot r
d
C = [xd, yd]
>, your control law took the form:
τ = J
T

K(r
d
C − rC ) + D(r˙
d
C − r˙ C )

. (1)
As we watched in the lecture, this method effectively controlled the position of the foot using a virtual
spring-damper in task-space. However, the Jacobian J =
∂rC
∂q was the only part of this control law that
required information about the model! This homework will explore methods to improve the performance
of the controller using more information about the model. Specifically, we will use information about its
dynamics.
As we have seen in class, the dynamics of the leg are given by the configuration-space equations of motion:
M(q)¨q + C(q, q˙) + G(q) = τ (2)
After a bit of algebraic manipulation, these equations can be rearranged into the operational-space equations of motion to describe the dynamics of the end-effector:
Λ(q) ¨rC + µ(q, q˙) + ρ(q) = F (3)
where Λ(q) is the effective mass felt at the foot, µ(q, q˙) gives the coriolis and centripetal forces on the foot,
and ρ(q) gives the gravity force felt on the foot. Formula for these quantities are given below:
Λ(q) = (JM−1J
T
)
−1
(4)
µ(q, q˙) = ΛJM−1 C − ΛJ˙ q˙ (5)
ρ(q) = ΛJM−1 G (6) ✓1
✓2
m1,I1
m2,I2
(0, 0)
ˆj
ˆi
B
C
l1
l2
c1
c2
Figure 1: Double pendulum and parameter definitions.
1
In this assignment, you will explore the use of an extended version of Eq. 1 given by:
τ = J
>

Λ

¨r
d
C + K(r
d
C − rC ) + D(r˙
d
C − r˙ C )


+ µ + ρ

. (7)
Under mild assumptions, it can be show that the foot will converge to the desired trajectory with this control
law.
1. Download the derive_eqns.m code. Using the simulate_pend.m code as a simulator, implement the
control law in Eq. 7 in controller.m. The script derive_eqns.m creates a number of useful functions
what will help in implementing the controller once you specify kinetic/potential energy and generalized
forces.
With your operational space controller, run the circular trajectory tracking simulation. You should
use parameters:
r
d
C (t) = 
0.5 + 0.25 cos(ωt)
−1 + 0.25 sin(ωt)

(8)
ω = 2π rad/s (9)
K =

50 0
0 50
(10)
D =

5 0
0 5
(11)
q|t=0 =

π/6
π/6

(12)
q˙|t=0 = [0, 0]> (13)
Turn in a plot of of x, y, xd, and yd versus time.
2. Repeat step 2. using modified control laws:
• τ = J
T

Λ

¨r
d
C + K(r
d
C − rC ) + D(r˙
d
C − r˙ C )


+ µ

• τ = J
T

Λ

¨r
d
C + K(r
d
C − rC ) + D(r˙
d
C − r˙ C )


+ ρ

• τ = J
T

Λ

K(r
d
C − rC ) + D(r˙
d
C − r˙ C )


+ µ + ρ

Turn in a plot of of x, y, xd, and yd versus time for each case. Also, include a short description of what
is neglected in each controller, and how that relates with the observed performance. For comparison,
you probably need to try different trajectory speed, link masses, or feedback gains.
3. Turn in your controller.m and derive_eqns.m code