Hybrid dynamics

[shamilmamedov]

Does pinocchio support hybrid dynamics? If it doesn't, is there a simple way to implement it on top of pinocchio? To give a bit of context, I have a serial chain floating base robot. I want to assign acceleration to the floating base joint and compute the accelerations of other joints using forward dynamics.

[jcarpent]

Good question. You should be able to do that with the constrained dynamics functions by setting the desired acceleration of the floating base.
In general, it would be nice to add the feature overall.
In which context do you use this feature?

[shamilmamedov]

@jcarpent, thank you for quick reply! Is there an example or unittest that I can get inspiration from?

I am using this feature in the context of deformable object manipulation where an object is attached to the end-effector of the robot. For time being I assume that I can directly assign the end-effector acceleration, thus need to solve hybrid dynamics problem.

[jcarpent]

You can take inspiration from

pinocchio/unittest/contact-dynamics.cpp

Lines 303 to 360 in c7530c9

	BOOST_AUTO_TEST_CASE (timings_fd_llt)
	{
	using namespace Eigen;
	using namespace pinocchio;
	pinocchio::Model model;
	pinocchio::buildModels::humanoidRandom(model,true);
	pinocchio::Data data(model);
	#ifdef NDEBUG
	#ifdef _INTENSE_TESTING_
	const size_t NBT = 1000*1000;
	#else
	const size_t NBT = 100;
	#endif
	#else
	const size_t NBT = 1;
	std::cout << "(the time score in debug mode is not relevant) " ;
	#endif // ifndef NDEBUG
	VectorXd q = VectorXd::Ones(model.nq);
	q.segment <4> (3).normalize();
	pinocchio::computeJointJacobians(model, data, q);
	VectorXd v = VectorXd::Ones(model.nv);
	VectorXd tau = VectorXd::Zero(model.nv);
	const std::string RF = "rleg6_joint";
	const std::string LF = "lleg6_joint";
	Data::Matrix6x J_RF (6, model.nv);
	J_RF.setZero();
	getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
	Data::Matrix6x J_LF (6, model.nv);
	J_LF.setZero();
	getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);
	Eigen::MatrixXd J (12, model.nv);
	J.topRows<6> () = J_RF;
	J.bottomRows<6> () = J_LF;
	Eigen::VectorXd gamma (VectorXd::Ones(12));
	model.lowerPositionLimit.head<7>().fill(-1.);
	model.upperPositionLimit.head<7>().fill( 1.);
	q = pinocchio::randomConfiguration(model);
	PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
	SMOOTH(NBT)
	{
	pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.);
	}
	timer.toc(std::cout,NBT);
	}

[shamilmamedov]

@jcarpent thank you for the reference!

[shamilmamedov]

For those of you who are interested in computing hybrid dynamics using Pinocchio, I have discovered a method to do so using inverse dynamics implementation. The details of this method are explained in Roy Featherstone's book. Additionally, I have written a short blog post where I briefly outline the method and provide a simple code snippet demonstrating how to implement it in Pinocchio.

[jcarpent]

Very nice initiative @shamilmamedov. Your blog post might be very helpful for people.

We may already think to implement such a solution in C++ directly for efficiency reasons.
@shamilmamedov Would you like to be involved in this task?

[shamilmamedov]

@jcarpent, I would like to, however, unfortunately, I am not an expert in C++ nor in spatial algebra