Current implementation uses an "admittance gain" of 1.0 (torqueToVelocityGain) to convert joint torques to joint velocities:
|
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
