Applying external push on the body of robot

MiladSh · December 16, 2020, 10:51am

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

ytazz · December 17, 2020, 3:00am

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;
}

};

nakaoka · January 8, 2021, 4:24am

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.

nakaoka · January 8, 2021, 4:37am

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.

ytazz · January 11, 2021, 3:51am

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::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.

Ahmadreza · August 25, 2021, 11:02pm

Hi @nakaoka ,
I am trying to do something similar to what @MiladSh mentioned, from a controller other than SimpleController.But first, I have tried to implement the sample SR1WalkPush.py
on SR1Walk.cnoid. I have encountered this error:

Warning: Failed to run the python script.
Traceback (most recent call last):
  File "<string>", line 10, in <module>
  File "<string>", line 5, in pushWaist
AttributeError: 'Boost.Python.function' object has no attribute 'findItem

It could be great if you could advise.

Furthermore, assuming I successfully run the sample, for the case I want to run it from another controller I have the following questions as well:

I wanna know what parts of SR1WalkPush.py should be changed to be able to use it for a different controller.
I want to apply different external forces at different times of a specific motion (for example 200N at t=2sec, -300N at t=5sec, etc.).

Thank you
Ahmadreza

nakaoka · August 26, 2021, 10:51am

Are you using Choreonoid 1.7.0? You can run the script on it, but it’s better to use the latest development version.
The python bindings implemented with boost.python is deprecated. Please use the pybind11 version of the python bindings, which is used by default in Choreonoid 1.7.0. The latest development version of Choreonoid only supports the pybind11 version. The boost.python version was removed from it.
It seems that you run the script by specifying it as a command-line option, but it is not designed to be used in that way. The script must be executed during the simulation. To do it, you first have to load the script as a Python script item, and execute the script using the script execution button or the context menu of the script item during the simulation.

Sorry, I’m not sure I understand your question. Do you mean that you have multiple robots and you want to apply force to the second and subsequent robots?

I think the python script can be improved to support it by using a timer object. However, the time cannot strictly be synchronized between the python script which is executed in the main thread and the simulation executed in the dedicated thread. If you want to apply the force at the exact timing you intend in the virtual world time, you have to use the robot controller. You can apply the arbitrary force to the arbitrary point like the script from a SimpleController.