Dynamic simulation of manipulator/The force control of manipulator

[WeiJunQ]

Hello, I'm a beginner for DART, and I find that DART have a good result on robot dynamics simulation. I'm taking over a project about the force control using a six DOF industrial robot arm(This problem involves the dynamics of robot) recently, I want to find some relevant examples from this repository but I can't find something useful, The most examples only talk about human or humanoid robots and have few instance that involve dynamics control of the manipulator(Maybe I didn't find).

I really don't know what to do. Could you give me some useful help or suggestions about this matter？ Thanks!

[mxgrey]

You can find an example of an operational space controller being used on a model of a KUKA arm in examples/osgExamples/osgOperationalSpaceControl. That example program requires OpenSceneGraph and allows you to drag/drop the target with your mouse.

There's an older alternative version that does not require OpenSceneGraph in examples/operationalSpaceControl.

The dominoes tutorial also illustrates operational space control on the KUKA arm model. In that tutorial, the operational space controller is used to make the robot arm knock over the first domino.

Operational space control is a force control technique that takes into account error in the desired pose, velocity, and applied force of the end effector. It's likely that if you're doing force control, you'll want to use something similar to an operational space controller.

[WeiJunQ]

Thank you for the super fast answer.

I have read this dominoes tutorial, but I don't understand the procedure and principle about the dominoes tutorial Particularly this part about operational space controller, At least for a beginner, I think that the annotation of this program is not very detailed, and I don't know how apply the operational space controller to my practical project.

Could you give me some explanation or suggestions about the operational space controller and that it is how to achieve the force control? Can use DART to realize that stop the motion of the manipulator after touch(contact-stop)? Are there any relevant resources and example program?

Thank you again for your help！

[mxgrey]

If we assume that the arm is already at the desired configuration, and you simply want to apply a specific force to an object, starting from zero velocity, then there are only two important functions in the DART API.

1. dart::dynamics::Skeleton::getCoriolisAndGravityForces() will give you an Eigen::VectorXd containing the joint forces necessary to counteract gravitational and Coriolis effects on your robot arm. The indices in the vector match the indices of the generalized coordinates in the Skeleton. Use the values in this vector as a baseline for the generalized forces that you will command to the arm.

2. dart::dynamics::BodyNode::getLinearJacobian() or dart::dynamics::EndEffector::getLinearJacobian(). This will give you a 3 x N Jacobian matrix for the BodyNode or EndEffector that you call it on. Call this function on the BodyNode or EndEffector that you want to use to apply the force. You can then use the transpose of this matrix to get the joint forces needed to apply a desired linear force.

Altogether, your code might look something like this:

void applyControlForce(
  const dart::dynamics::SkeletonPtr& robot,
  const dart::dynamics::JacobianNode* endEffector,
  const Eigen::Vector3d& force)
{
  robot->setForces(robot->getCoriolisAndGravityForces() + endEffector->getLinearJacobian().transpose()*force);
}

Note that JacobianNode is a base class of both BodyNode and EndEffector, so you can pass either of those class types into this function.

There is an important caveat for this very simple approach: Using getLinearJacobian().transpose()*force will not keep the end effector moving in a straight line. If it is already in contact, and that contact point does not move or slip, then it will continuously apply the specified force. But if the end effector is able to move for any reason, then it will not move in a straight line.

The benefit of the more complicated operational space controller is that it uses the pseudo-inverse Jacobian to ensure that the end effector stays on a desired trajectory, in addition to applying a desired force. You will most likely want to use an operational space controller, but maybe this simpler approach will help you get started.

To actually understand how an operational space controller works, I would recommend looking for a paper on the topic. I doubt I could do it justice in a GitHub comment.

[WeiJunQ]

Thanks for the fast reply! I will try it. I'm very happy to see some paper how an operational space controller works and how to achieve the accurate force control with DART. If you can find it, please let me know at the first time.

[WeiJunQ]

Hello, I can't find some relevant definitions about mRI in the head file MyWindow.hpp. The detailed code is as follows:

void MyWindow::drawWorld() const
{
// Draw the target position
if (mRI)
{
mRI->setPenColor(Eigen::Vector3d(0.8, 0.2, 0.2));
mRI->pushMatrix();
mRI->translate(mTargetPosition);
mRI->drawEllipsoid(Eigen::Vector3d(0.05, 0.05, 0.05));
mRI->popMatrix();
}

// Draw world SimWindow::drawWorld(); }

What role does each function play in the construction, just like `pushMatrix` and `popMatrix` and so on？ I find haven't detailed explanations about this Class and function in the API document.

[mxgrey]

RI stands for Render Interface. The code you're looking at just has to do with displaying graphics in a GLUT window, and isn't a particularly important feature of DART.

This is old code that I don't really recommend working off of. If you want to render DART simulation results, I recommend using the dart-gui-osg library instead, which is object-oriented and should be more intuitive to use.

[WeiJunQ]

OK. Another problem is that I want to know what meanings the 1.0/1000-second interval stands for in the world->setTimeStep(1.0/1000);. I try to change the TimeStep, in order to reduce the speed of motion simulation I changed 1.0/1000 to 1.0/500, but I found that the simulation error has occurred, in which the robotic arm disappeared. What should I do if I want to reduce the speed of motion simulation of the manipulator. Thanks!

[mxgrey]

There shouldn't be any reason for a robot arm to "disappear" in a DART simulation the way it might in other simulators like ODE, because DART uses generalized coordinates which implicitly enforce joint constraints.

We can't really diagnose what might be wrong with your simulation unless you're able to point us at the code you've written.

[WeiJunQ]

I use the example of operationalSpaceControl in examples/operationalSpaceControl/Main.cpp.

In order to reduce the speed of motion simulation of the manipulator, I just changed 1.0/1000 to 1.0/100(not 1.0/500,I type a mistake in the above section). My source code is as follows：

  // create and initialize the world
  Eigen::Vector3d gravity(0.0, -9.81, 0.0);
  world->setGravity(gravity);
  // world->setTimeStep(1.0/1000);
  world->setTimeStep(1.0/100);

When TimeStep is 1.0/500, he can still work properly. But when the value of TimeStep is set to 100, he has the following mistakes:

[mxgrey]

This is a subtle distinction, but the arm isn't disappearing: the base (which is part of the arm) is still there. What's happening is the control commands that you're sending to the arm are unstable for the size of the timestep that you're requesting, and they quickly reach infinite or NaN values. When the joint positions reach NaN values, the transforms of each body can no longer be computed, and so they don't get rendered.

The stability of a discrete simulation with a feedback controller is a function of both the control gains and the size of the time step. Control gains that are stable for a small time step can be unstable for a large time step. DART is doing the correct thing for what you're asking: you are setting up a simulation that is mathematically numerically unstable by making a large time step while using control gains that were tuned for a small time step. In the Controller.cpp file of the example, you can change the mKp and mKv values on lines 52-53 to 500.0 and 100.0 respectively, to go along with the change in the time step. You will find that the simulation is once again stable.

Identifying the right gains to use for a given simulation can be very difficult. When you see this type of instability in a discrete time-stepping simulation, it usually means that the derivative gain (in this case, mKv) is too high, but it can also be caused by the proportional gain being too high, or by the gains not being good fits for each other.

I'd recommend finding a book on numerical methods to better understand these mathematical concepts. If you're a student, I'd encourage you to take some classes on control theory, robot mechanics, and numerical methods. The GitHub issue tracker is not an ideal place for providing instruction in math and control theory. It's also somewhat off topic since this has to do with fundamental mathematics, and it's not an issue that is specific to DART, but I won't shut down this discussion since it may be informative to other users.

[mxgrey]

Although going back a little bit:

What should I do if I want to reduce the speed of motion simulation of the manipulator.

I'm not exactly sure what you mean by this. Are you saying that you want the arm to respond less aggressively when you move around the target? To do that, you should be adjusting the mKp and/or mKv variables that are mentioned above. The time step corresponds (roughly) to a trade off between simulation accuracy and simulation run-time.

A smaller time step will typically make a simulation more accurate, but it will take longer for the simulation to make progress. A larger time step will allow the simulation to finish more quickly, but it will typically make the simulation less accurate. You can think of the time step as being similar to the step size of a Riemann sum.

Again, this is fundamental mathematics and control theory, so I would recommend reading textbooks or taking classes in these subjects to better understand the concepts.

[mxgrey]

Maybe the takeaway from this thread is that we should try to make some tutorials that explain and demonstrate fundamental concepts in numerical methods and control theory.

Those could be helpful for @karenliu's classes and for robotics students in general who are new to simulation and control.

[WeiJunQ]

What should I do if I want to reduce the speed of motion simulation of the manipulator.

I'm not exactly sure what you mean by this. Are you saying that you want the arm to respond less aggressively when you move around the target? To do that, you should be adjusting the mKp and/or mKv variables that are mentioned above. The time step corresponds (roughly) to a trade off between simulation accuracy and simulation run-time.

You may misunderstand what I want to express.I just wanted the manipulator to look slow in the simulation environment to make it easier for us to see some of the details of the motion. It's not the speed at which it reaches the target point. Thanks!

[WeiJunQ]

@mxgrey hello,
Now I want to achieve the end effector of the arm to move along a specific direction at a suitable speed, which will stop moving when the contact force between the end effector and the object reach 2N, what should I do? I don't know which function should be used to achieve that the end effector moves along the specified direction at a suitable speed. In addition, How do I get the contact force between the end effector of the manipulator and the target object? Could you give me some suggestions and guidance? Sincerely look forward to your early reply. Thank you!

[mxgrey]

I'll go back a bit since I've been away for a little while.

I just wanted the manipulator to look slow in the simulation environment to make it easier for us to see some of the details of the motion.

You can't ask the simulation to slow down how fast it computes the results, so the simplest way to do this is to add a sleep to the timestep of the world that you're viewing. For example, you could add usleep(100000); to the timestepping function to have it stop for 1/10 of a second each time step. You can tweak that number to adjust how slow the simulation runs. There are better ways to do this (e.g. use a variable that can be modified by the user during runtime), but doing some kind of sleep is the simplest way to slow things down.

A more elegant way to observe the behavior of a system is to record the states and use the keyboard to pause and rewind a simulation. The osgSoftBodies example demonstrates how to do this, but that's probably more work than it's worth for what you're trying to accomplish.

I want to achieve the end effector of the arm to move along a specific direction at a suitable speed, which will stop moving when the contact force between the end effector and the object reach 2N, what should I do?

This is quite literally the exact purpose of an operational space controller: It lets you specify desired position, desired velocity, and desired applied force for an end effector, and it will compute the joint torques needed to achieve those things. You should really look into articles on operational space control if the DART examples aren't clear enough.

How do I get the contact force between the end effector of the manipulator and the target object?

You don't really need this for an operational space controller to work, but you could use this information to confirm that it is applying the correct force. It might look something like this:

dart::dynamics::BodyNode* endEffector = robot->getBodyNode("end effector");
const dart::collision::CollisionResult& result = world->getLastCollisionResult();
Eigen::Vector3d F_contact = Eigen::Vector3d::Zero();
for(const auto& contact : result.getContacts())
{
  for(const auto& shapeNode : endEffector->getShapeNodesWith<dart::dynamics::CollisionAspect>())
  {
    if(shapeNode == contact.collisionObject1->getShapeFrame() ||
       shapeNode == contact.collisionObject2->getShapeFrame())
    {
      F_contact += contact.force;
    }
  }
}

Then F_contact will contain the contact forces that are being applied to the BodyNode called "end effector".

Alternatively, if the only external forces (i.e. everything except for joint forces) that are being applied to the end effector are due to collision, you can simplify it to this:

const Eigen::Vector3d F_contact = robot->getBodyNode("end effector")->getExternalForceGlobal().block<3,1>(3,0);

[WeiJunQ]

Great. Thank you for your reply. Your reply to me too useful, maybe I should carefully study this operational space controller. If you can have relevant reference articles or document, please be sure to tell me at the first time. Of course, I will continue to find the relevant articles in this area.

[mxgrey]

I'm going to go ahead and close this issue. I think #1037 captures the essential takeaway message of this thread.

[adamconkey]

@mxgrey Your suggestion was very helpful to me. I have code that does exactly this kind of computation of end-effector forces, and I'm porting from release 5.1 to 6.7. I could not for the life of me figure out how to get the collisions correctly. I will add that for it to work correctly for me, I had to check which collision object on the contact was associated with the end-effector, and modify the sign of the force accordingly:

for (const auto &shape : end_effector->getShapeNodesWith<CollisionAspect>())
{
    if (shape == contact.collisionObject1->getShapeFrame())
        force += contact.force;
    else if (shape == contact.collisionObject2->getShapeFrame())
        force -= contact.force;
}