Use Robot Dynamics Equation to convert Joint Torques to Joint Velocities

[Sharwin24]

Current implementation uses an "admittance gain" of 1.0 (torqueToVelocityGain) to convert joint torques to joint velocities:

potential_fields/potential_fields_library/src/pfield/pfield.cpp

Lines 279 to 334 in 6e7f1b3

	Eigen::VectorXd PotentialField::convertJointTorquesToJointVelocities(
	const Eigen::VectorXd& jointTorques, const std::vector<double>& jointAngles,
	const std::vector<double>& prevJointVelocities, const double dt) const {
	// TODO(Sharwin24): Fix this method and figure out why it's unstable
	// https://github.com/argallab/pfields_2025/issues/23
	const bool useRobotDynamicsEquation = false;
	if (!this->pfKinematics || !useRobotDynamicsEquation) {
	// If we don't have access to PFKinematics, use a simple proportional mapping
	return this->torqueToVelocityGain * jointTorques;
	}
	// Retrieve Mass Matrix, Coriolis, and Gravity
	Eigen::MatrixXd M = this->pfKinematics->getMassMatrix(jointAngles);
	// C contains the result of C * q_dot since it's simpler to compute
	Eigen::VectorXd C = this->pfKinematics->getCoriolisVector(jointAngles, prevJointVelocities);
	Eigen::VectorXd G = this->pfKinematics->getGravityVector(jointAngles);
	// Add joint damping to stabilize the motion (M*q_ddot = Tau - D*q_dot)
	// A simple constant damping prevents oscillation from the conservative potential field
	const double dampingGain = 20.0;
	Eigen::VectorXd prevJointVels = Eigen::Map<const Eigen::VectorXd>(prevJointVelocities.data(), prevJointVelocities.size());
	Eigen::VectorXd dampingTorque = dampingGain * prevJointVels;
	Eigen::VectorXd netTorques = jointTorques - C - G - dampingTorque;
	Eigen::VectorXd jointAccelerations;
	try {
	jointAccelerations = M.llt().solve(netTorques);
	}
	catch (const std::exception& e) {
	std::cerr << "Error solving for joint accelerations: " << e.what() << std::endl;
	jointAccelerations = netTorques; // Fallback to direct mapping
	}
	// --- Integrate joint accelerations to get joint velocities ---
	Eigen::VectorXd jointVelocities = prevJointVels;
	if (isPositiveFinite(dt)) {
	jointVelocities += jointAccelerations * dt;
	}
	else {
	return jointAccelerations; // If dt is invalid, fallback to direct mapping
	}
	// --- Apply Joint Velocity/Acceleration Limits ---
	if (this->pfKinematics) {
	const auto& model = this->pfKinematics->getModel();
	for (int i = 0; i < jointVelocities.size() && i < model.velocityLimit.size(); ++i) {
	double limit = model.velocityLimit[i];
	// Pinocchio sometimes sets limits to infinity or very large values if undefined
	if (limit > 1e-3 && limit < 1e10) {
	jointVelocities[i] = std::clamp(jointVelocities[i], -limit, limit);
	}
	}
	}
	return jointVelocities;
	}

---

This should be changed to use the robot dynamics equation that incorporates the robot's dynamics and the gravitational + coriolis forces depending on the robot's joint positions, velocities and acceleration $[q, \dot{q}, \ddot{q}]$:

$$ \tau = \underbrace{M(q)}_{\text{Mass-Inertia Matrix}} \ddot{q}​ + \underbrace{C(q,\dot{q}​)}_{\text{Coriolis Force}} \dot{q}​ + \underbrace{g(q)}_{\text{Gravitational Force}} $$

---

Luckily, Pinocchio has a few helper libraries for us:

#include <pinocchio/algorithm/rnea.hpp> // Required for gravity vector
#include <pinocchio/algorithm/crba.hpp> // Required for mass matrix
#include <pinocchio/algorithm/ccrba.hpp> // Required for coriolis matrix

pinocchio::Model model = /* Load model from URDF */
pinocchio::Data data = pinocchio::Data(model);

Eigen::VectorXd q = /* current joint configuration */

// Computes M matrix and stores it in data.M
pinocchio::crba(model, data, q); 
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose();

// Computes gravity vector g(q)
// Pinocchio's rnea with q_dot=0, q_ddot=0 returns g(q)
Eigen::VectorXd gravity_full = pinocchio::computeGeneralizedGravity(model, data, q);

// Computes C(q, q_dot) * q_dot
Eigen::VectorXd q = jointAngles; // Aligned with model.nq
Eigen::VectorXd v = jointVelocities;
Eigen::VectorXd coriolis_full = pinocchio::computeCoriolisTerm(model, data, q, v);

---

Unfortunately, this approach is either very sensitive to tuning or has a few implementation/approach flaws that make it difficult to realistically use. Thankfully, we likely don't need to use this equation anyways since admittance gain seems to work well enough

[Sharwin24]

Since this is implemented but not yet working, I'll reopen the issue.