Applying external push on the body of robot

Hello,

I’m simulating a push recovery controller on a humanoid robot in Choreonoid and sometimes ago I was using follwoing lines for applying external force, however it does not work with recent version of the Choreonoid.

currentBodyItem = RootItem::instance()->findItem(“World/MyRobot”);
currentBodyItem1 = RootItem::instance()->findItem(“World/MyRobot”);

activeSimulator = SimulatorItem::findActiveSimulatorItemFor(currentBodyItem);
activeSimulator->setExternalForce(currentBodyItem1, waistLink, point, force, 0.5);

Would you please guide me how can I apply external force on a specific link in c++ in the recent version of Choreonoid?

Thank you very much.

Best wishes,
Milad Shafiee

Hi.
If you are implementing a SimpleController plugin to control your robot,
one way is to apply external forces to the robot from the controller.
The following is an example code. It works in my case.
I’m not sure if this is a right way to do it, though.

class MyController : public SimpleController{
public:
Body* ioBody;
int count;

virtual bool initialize(SimpleControllerIO* io){
	
	//...
	
	ioBody = io->body();
	io->enableOutput(ioBody->link(0), cnoid::SimpleControllerIO::LINK_FORCE);
	count = 0;
	
	//...
	
	return true;
}

virtual bool control()	{
	// add disturbance
	if(std::abs(count - 1000) < 10){
		ioBody->link(0)->f_ext() = Vector3(1000.0, 0.0, 0.0);
	}
	else{
		ioBody->link(0)->f_ext() = Vector3(0.0, 0.0, 0.0);
	}
	
	count++;
	
	return true;
}

};

Thanks ytazz.
Your answer is a right way when you use a SimpleController.
However let me point out one thing. In Choreonoid’s internal library, the torque element of the enxternal force applied to a link must be described in the global coordinate. That means the torque position must be origin.
Therefore when you apply force Vector3(1000.0 , 0.0, 0.0) to the origin of the link, the following code is right:

auto link = ioBody->link(0);
Vector3 f(1000.0, 0.0, 0.0);
link->f_ext() = f;
link->tau_ext() = link->p().cross(f);

link->p() is the global position of the link origin. If you want to apply the force to another position, replace the p value with it.

The link class provides a function to do the above thing. For example, if you want to apply the force to the link local position (1, 2, 3), you can write as follows:

link->setExternalForceAtLocalPosition(f, Vector3(1, 2, 3));

By the way, using the torque value around the global origin was customary in the laboratory I belonged to, but is it a standard way generally? Sometimes I get confused by this way of describing torque. It may be more general to use the center of mass or the link origin as the torque position. If you have any suggestions, I would be glad to hear them.

MiladSh may want to apply the force from a code part other than a SimpleController. There is a python sample similar to your C++ code. The sample is sample/python/SR1WalkPush.py. For example, invoke Choreonoid by the following command:

choreonoid sample/SimpleController/SR1Walk.cnoid --python-item sample/python/SR1WalkPush.py

Then start the simulation. When you click the script execution button, an external force is applied to the robot, and the robot probably falls down at that time.

I confirmed that this python sample still works correctly with the latest Choreonoid development version. The functions used in the python code are just warppers of the corresponding C++ functions, so if you write the same code in C++, you will get the same result.

Thanks. Maybe I did not realize the difference because the robot was fairly close to the origin.

By the way, I cannot find setExternalForceAtLocalPosition in the source code.
I pulled from the master branch of choreonoid/choreonoid.

In addition, cnoid::SimpleControllerIO::LINK_FORCE seem to be obsolete,
I had to change it to cnoid::link::LinkExtWrench ( I hope this is correct).
Until recently, I’ve been pulling from s-nakaoka/choreonoid, and I switched to choreonoid/choreonoid.

I hope there will be a more up-to-date documentation of enums related to enableIO apis.

  • I cannot find much benefit in expressing external forces around the origin of the global frame,
    though I cannot find any possible problem caused by doing so either.
    I think in the API design, expressing external forces around the origin of the rigid body is convenient,
    while expressing them around the center of mass is best suited to internal computation.