CODEWITHSUNDEEP
controllerdrake
Maggie Wang
Maggie Wang

Reputation: 13

Velocity Controller for Kuka Robot

I'm trying to make my Kuka end effector go to a desired goal using a control Lyapunov function. I'm following this paper: http://skrifo.juxi.net/papers/zhuang2019iros.pdf

The Lyapunov function is here, and this is its partial derivatives wrt the joint angles.

So far, the controller seems to be doing some wacky things--the Lyapunov function doesn't seem to be decreasing at all time steps [assert(V <= prev_V + 0.001) fails!]. The Lyapunov function seems right, but I think the issue comes from calculating the partial derivatives. The Jacobian calculation makes sense in my head (I want the output to be in the end effector frame), but something seems clearly wrong:

jacobian = robot.CalcJacobianSpatialVelocity(
    context=differential_ik.GetRobotContext(), with_respect_to=JacobianWrtVariable.kQDot,
    frame_B=robot.world_frame(),                    # frame on which point Bi is fixed
    p_BP=[0.0, 0.0, 0.0],                           # position vec from frame_B's origin to points Bi
    frame_A=robot.GetFrameByName("iiwa_link_7"),    # frame that measures v_ABi
    frame_E=robot.GetFrameByName("iiwa_link_7"))    # frame that v_ABi is expressed on input 
                                                    # and frame that Jacobian J_V_ABi is expressed on output

[I added GetRobotContext() to the DifferentialIK class but could have used GetSubsystemContext()]

I'm setting the velocity in Drake with q = q_prev + time_step * q_dot and then differential_ik.SetPositions() on q.

Here is the smallest working code I could create to replicate my issue (sorry it's still kind of large): https://gist.github.com/maggi3wang/1cbb02a0adc2cec85915f856ab3d1702#file-small_collect_data-py-L174

Any help would be greatly appreciated!

Upvotes: 1

Views: 357

Answers (1)

Hongkai Dai
Hongkai Dai

Reputation: 2766

I would say the quickest approach to debug your program is to check if ∂V / ∂θ is computed correctly (which I believe is the variable pdV in your code). You could compute this gradient through numerical difference, and then compare the numerical gradient with your gradient.

I tried to derive the control law below:

Say you want your ᵂX⁷ ∈ SE(3) (pose of link 7 measured and expressed in the world frame) to match with some desired pose ᵂX⁷_des, and you define your Lyapunov function as

V = |ᵂX⁷ - ᵂX⁷_des|_F

where I use | |_F to denote the Frobenius norm. I will use ᵂR⁷ to denote the rotation matrix, and ᵂp⁷ to denote the position of link 7 in the world frame.

This Lyapunov function is equivalent to 

V = |ᵂR⁷ - ᵂR⁷_des|_F + (ᵂp⁷ - ᵂp⁷_des)ᵀ(ᵂp⁷ - ᵂp⁷_des)
  = trace((ᵂR⁷ - ᵂR⁷_des)ᵀ(ᵂR⁷ - ᵂR⁷_des)) + (ᵂp⁷ - ᵂp⁷_des)ᵀ(ᵂp⁷ - ᵂp⁷_des)
  = 6 - 2*trace(ᵂR⁷_desᵀ * ᵂR⁷) + (ᵂp⁷ - ᵂp⁷_des)ᵀ(ᵂp⁷ - ᵂp⁷_des)

Your goal is to compute the gradient ∂V / ∂θ. To compute this partial derivative, you need the partial derivative ∂ᵂR⁷ / ∂θ and ∂ᵂp⁷ / ∂θ. When you compute the Jacobian from CalcJacobianSpatialVelocity, the translational part of this Jacobian is ∂⁷pᵂ / ∂θ (BTW I am more used to express the position/velocity in world frame instead of link 7's frame), but the rotational part of this Jacobian is not ∂ᵂR⁷ / ∂θ. Suppose you called

jacobian = robot.CalcJacobianSpatialVelocity(
    context=differential_ik.GetRobotContext(), with_respect_to=JacobianWrtVariable.kQDot,
    frame_B=robot.GetFrameByName("iiwa_link_7"),
    p_BP=[0.0, 0.0, 0.0],
    frame_A=robot.world_frame(),
    frame_E=robot.world_frame())

, the rotational part of this Jacobian is ∂ ᵂω⁷/∂ θ_dot, where ᵂω⁷ is the angular velocity of link 7 measured and expressed in the world frame. In order to compute ∂ᵂR⁷ / ∂θ, we use the following equation

∂ᵂR⁷ / ∂θ = ∂ᵂR_dot⁷ / ∂θ_dot
          = ∂(⌊ᵂω⁷⌋ₓ ᵂR⁷) / ∂θ_dot
          = (∂⌊ᵂω⁷⌋ₓ / ∂θ_dot ) * ᵂR⁷

in the first line of the equation, I took time derivative of both the numerator and the denominator. In the second line of the equation, I use the equation ᵂR_dot⁷ = ⌊ᵂω⁷⌋ₓ ᵂR⁷, where ⌊ᵂω⁷⌋ₓ represent the skew-symmetric matrix of the angular velocity ᵂω⁷ (BTW, I forgot if it is ᵂR_dot⁷ = ⌊ᵂω⁷⌋ₓ ᵂR⁷ or ᵂR_dot⁷ = ᵂR⁷⌊ᵂω⁷⌋ₓ. If one doesn't work, you could try the other) The last line of the equation holds by taking the gradient of a product, and ∂ᵂR⁷ / ∂θ_dot = 0.

Upvotes: 1

Related Questions