fzi-forschungszentrum-informatik / cartesian_controllers

A set of Cartesian controllers for the ROS1 and ROS2-control framework.
BSD 3-Clause "New" or "Revised" License
376 stars 109 forks source link

A question about the algorithm #19

Closed Csp0920 closed 2 years ago

Csp0920 commented 3 years ago

Hello, I have a question about the forward dynamics algorithm. I have read your paper and PPT. But I still don't know why forward dynamic equation can be simplified to: q'' = H^(-1) J^T f; Doesn't it matter to get rid of the other two: V(q,q') and G(q) ?

G(q) can be compensated by the joint controller? What about V(q,q')?

And if the H and J are the Inertia matrix and Jacobian matrix of real robot ?

Looking forward for your reply, Thank you very much.

stefanscherzinger commented 3 years ago

Hi @Csp0920

Doesn't it matter to get rid of the other two: V(q,q') and G(q) ?

The simplification is a design choice. The overall idea is to use forward simulations to obtain a direct mapping from end-effector wrench space to joint configuration space. Our goal is to achieve a mostly linear behavior in operational space. That means we want the robot to react with linear end-effector motion to virtual force-torque vectors in operational space. To achieve this behavior, we drop everything that disturbs. Take e.g. the gravitational component. If we included that, our virtual robot would sink to the ground. We don't want that. Similarly, when including the Coriolis and Centrifugal terms, our virtual robot would behave differently, depending on whether it moves fast or slow. We don't want that either.

And if the H and J are the Inertia matrix and Jacobian matrix of real robot ?

We use the Manipulator Jacobian J of the real robot! That assures that our robot can carry out the simulated motion. The interesting part is what happens through using a specially-conditioned Inertia matrix H. Even if we had realistic values for H, which are difficult to obtain, there would be no benefits for using those. The simulation would be realistic, yes, but that is not what we want. We want linear behavior in operational space, and we achieve that with setting the end-effector of this virtual system to dominate the overall dynamics. Let me point you to another paper that evaluates this idea in more detail.

Csp0920 commented 3 years ago

Hi @Csp0920

Doesn't it matter to get rid of the other two: V(q,q') and G(q) ?

The simplification is a design choice. The overall idea is to use forward simulations to obtain a direct mapping from end-effector wrench space to joint configuration space. Our goal is to achieve a mostly linear behavior in operational space. That means we want the robot to react with linear end-effector motion to virtual force-torque vectors in operational space. To achieve this behavior, we drop everything that disturbs. Take e.g. the gravitational component. If we included that, our virtual robot would sink to the ground. We don't want that. Similarly, when including the Coriolis and Centrifugal terms, our virtual robot would behave differently, depending on whether it moves fast or slow. We don't want that either.

And if the H and J are the Inertia matrix and Jacobian matrix of real robot ?

We use the Manipulator Jacobian J of the real robot! That assures that our robot can carry out the simulated motion. The interesting part is what happens through using a specially-conditioned Inertia matrix H. Even if we had realistic values for H, which are difficult to obtain, there would be no benefits for using those. The simulation would be realistic, yes, but that is not what we want. We want linear behavior in operational space, and we achieve that with setting the end-effector of this virtual system to dominate the overall dynamics. Let me point you to another paper that evaluates this idea in more detail.

Hi, @stefanscherzinger I have read the literature you gave me, and I want to ask if my understanding is correct. "J" is the Jacobian matrix of the real robot that you have said. "H" is the Inertia matrix of the virtual robot model?

stefanscherzinger commented 3 years ago

"J" is the Jacobian matrix of the real robot

It is the manipulator Jacobian of the virtual robot. For its computation, we use the real robot's kinematics in combination with the simulated joint configuration at each step.

"H" is the Inertia matrix of the virtual robot model?

Yes.

Csp0920 commented 3 years ago

Because this is the first time I've known VMC. I want to know why the position, velocity and acceleration calculated by virtual model control are also effective for real robot.

Csp0920 commented 3 years ago

"J" is the Jacobian matrix of the real robot

It is the manipulator Jacobian of the virtual robot. For its computation, we use the real robot's kinematics in combination with the simulated joint configuration at each step.

"H" is the Inertia matrix of the virtual robot model?

Yes.

I have a question. The paper mention that "The experiment further shows, that with a significant end-effector mass dominance (γ = 10e3) the forward dynamics mapping converges to the Jacobian inverse and makes this mapping particularly suitable for closed-loop control in terms of linearity." If the goal of the experiment is to prove that "the forward dynamics mapping converges to the Jacobian inverse"? So why not use q'' = J^(-1) * f to compute q'', but use q''=H^(-1)J^Tf.

stefanscherzinger commented 2 years ago

@Csp0920

I was researching to combine the benefits of both worlds: The Jacobian Inverse's Linearity and the Jacobian Transpose's Stability :)