675 lines
22 KiB
C++
675 lines
22 KiB
C++
/*
|
|
* RBDL - Rigid Body Dynamics Library
|
|
* Copyright (c) 2011-2015 Martin Felis <martin@fysx.org>
|
|
*
|
|
* Licensed under the zlib license. See LICENSE for more details.
|
|
*
|
|
* This file defines functions that allows calling of the RBDL algorithms
|
|
* by providing input and output as raw double arrays. It eliminates the
|
|
* need of copying from Numpy values into temporary RBDL (C++) vectors and
|
|
* matrices. However it requires C++11 and must be compiled with -std=c++11
|
|
* (or -std=c++0x on older compilers).
|
|
*/
|
|
|
|
#include <rbdl/rbdl_math.h>
|
|
#include <rbdl/Dynamics.h>
|
|
|
|
namespace RigidBodyDynamics {
|
|
|
|
namespace Math {
|
|
|
|
// PTR_DATA_ROW_MAJOR :
|
|
// Specifies whether the data that is provided via raw double pointers is
|
|
// stored as row major. Eigen uses column major by default and therefore
|
|
// this has to be properly mapped.
|
|
#define PTR_DATA_ROW_MAJOR 1
|
|
|
|
#ifdef RBDL_USE_SIMPLE_MATH
|
|
typedef VectorNd VectorNdRef;
|
|
typedef MatrixNd MatrixNdRef;
|
|
#else
|
|
typedef Eigen::Ref<Eigen::VectorXd> VectorNdRef;
|
|
|
|
#ifdef PTR_DATA_ROW_MAJOR
|
|
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixNdRowMaj;
|
|
typedef Eigen::Ref<MatrixNdRowMaj> MatrixNdRef;
|
|
#else
|
|
typedef Eigen::Ref<Eigen::MatrixXd> MatrixNdRef;
|
|
#endif
|
|
|
|
#endif
|
|
|
|
RBDL_DLLAPI inline VectorNdRef VectorFromPtr (double *ptr, unsigned int n) {
|
|
#ifdef RBDL_USE_SIMPLE_MATH
|
|
return SimpleMath::Map<VectorNd> (ptr, n, 1);
|
|
#elif defined EIGEN_CORE_H
|
|
return Eigen::Map<VectorNd> (ptr, n, 1);
|
|
#else
|
|
std::cerr << __func__ << " not defined for used math library!" << std::endl;
|
|
abort();
|
|
return VectorNd::Constant (1,1./0.);
|
|
#endif
|
|
}
|
|
|
|
RBDL_DLLAPI inline MatrixNdRef MatrixFromPtr (double *ptr, unsigned int rows, unsigned int cols, bool row_major = true) {
|
|
#ifdef RBDL_USE_SIMPLE_MATH
|
|
return SimpleMath::Map<MatrixNd> (ptr, rows, cols);
|
|
#elif defined EIGEN_CORE_H
|
|
#ifdef PTR_DATA_ROW_MAJOR
|
|
return Eigen::Map<MatrixNdRowMaj> (ptr, rows, cols);
|
|
#else
|
|
return Eigen::Map<MatrixNd> (ptr, rows, cols);
|
|
#endif
|
|
#else
|
|
std::cerr << __func__ << " not defined for used math library!" << std::endl;
|
|
abort();
|
|
return MatrixNd::Constant (1,1, 1./0.);
|
|
#endif
|
|
}
|
|
|
|
}
|
|
|
|
RBDL_DLLAPI
|
|
void UpdateKinematicsCustomPtr (Model &model,
|
|
const double *q_ptr,
|
|
const double *qdot_ptr,
|
|
const double *qddot_ptr
|
|
) {
|
|
LOG << "-------- " << __func__ << " --------" << std::endl;
|
|
|
|
using namespace RigidBodyDynamics::Math;
|
|
|
|
unsigned int i;
|
|
|
|
if (q_ptr) {
|
|
VectorNdRef Q = VectorFromPtr(const_cast<double*>(q_ptr), model.q_size);
|
|
|
|
for (i = 1; i < model.mBodies.size(); i++) {
|
|
unsigned int lambda = model.lambda[i];
|
|
|
|
VectorNd QDot_zero (VectorNd::Zero (model.qdot_size));
|
|
|
|
jcalc (model, i, (Q), QDot_zero);
|
|
|
|
if (lambda != 0) {
|
|
model.X_base[i] = model.X_lambda[i] * model.X_base[lambda];
|
|
} else {
|
|
model.X_base[i] = model.X_lambda[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
if (qdot_ptr) {
|
|
VectorNdRef Q = VectorFromPtr(const_cast<double*>(q_ptr), model.q_size);
|
|
VectorNdRef QDot = VectorFromPtr(const_cast<double*>(qdot_ptr), model.qdot_size);
|
|
|
|
for (i = 1; i < model.mBodies.size(); i++) {
|
|
unsigned int lambda = model.lambda[i];
|
|
|
|
jcalc (model, i, Q, QDot);
|
|
|
|
if (lambda != 0) {
|
|
model.v[i] = model.X_lambda[i].apply(model.v[lambda]) + model.v_J[i];
|
|
model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]);
|
|
} else {
|
|
model.v[i] = model.v_J[i];
|
|
model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]);
|
|
}
|
|
// LOG << "v[" << i << "] = " << model.v[i].transpose() << std::endl;
|
|
}
|
|
}
|
|
|
|
if (qddot_ptr) {
|
|
VectorNdRef QDDot = VectorFromPtr(const_cast<double*>(qddot_ptr), model.qdot_size);
|
|
|
|
for (i = 1; i < model.mBodies.size(); i++) {
|
|
unsigned int q_index = model.mJoints[i].q_index;
|
|
|
|
unsigned int lambda = model.lambda[i];
|
|
|
|
if (lambda != 0) {
|
|
model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i];
|
|
} else {
|
|
model.a[i] = model.c[i];
|
|
}
|
|
|
|
if (model.mJoints[i].mDoFCount == 3) {
|
|
Vector3d omegadot_temp ((QDDot)[q_index], (QDDot)[q_index + 1], (QDDot)[q_index + 2]);
|
|
model.a[i] = model.a[i] + model.multdof3_S[i] * omegadot_temp;
|
|
} else {
|
|
model.a[i] = model.a[i] + model.S[i] * (QDDot)[q_index];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
RBDL_DLLAPI
|
|
void CalcPointJacobianPtr (
|
|
Model &model,
|
|
const double *q_ptr,
|
|
unsigned int body_id,
|
|
const Math::Vector3d &point_position,
|
|
double * G_ptr,
|
|
bool update_kinematics
|
|
) {
|
|
LOG << "-------- " << __func__ << " --------" << std::endl;
|
|
|
|
using namespace RigidBodyDynamics::Math;
|
|
|
|
// update the Kinematics if necessary
|
|
if (update_kinematics) {
|
|
UpdateKinematicsCustomPtr (model, q_ptr, NULL, NULL);
|
|
}
|
|
|
|
VectorNdRef Q = VectorFromPtr(const_cast<double*>(q_ptr), model.q_size);
|
|
MatrixNdRef G = MatrixFromPtr(const_cast<double*>(G_ptr), 3, model.qdot_size);
|
|
|
|
SpatialTransform point_trans = SpatialTransform (Matrix3d::Identity(), CalcBodyToBaseCoordinates (model, Q, body_id, point_position, false));
|
|
|
|
assert (G.rows() == 3 && G.cols() == model.qdot_size );
|
|
|
|
unsigned int reference_body_id = body_id;
|
|
|
|
if (model.IsFixedBodyId(body_id)) {
|
|
unsigned int fbody_id = body_id - model.fixed_body_discriminator;
|
|
reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
|
|
}
|
|
|
|
unsigned int j = reference_body_id;
|
|
|
|
// e[j] is set to 1 if joint j contributes to the jacobian that we are
|
|
// computing. For all other joints the column will be zero.
|
|
while (j != 0) {
|
|
unsigned int q_index = model.mJoints[j].q_index;
|
|
|
|
if (model.mJoints[j].mDoFCount == 3) {
|
|
G.block(0, q_index, 3, 3) = ((point_trans * model.X_base[j].inverse()).toMatrix() * model.multdof3_S[j]).block(3,0,3,3);
|
|
} else {
|
|
G.block(0,q_index, 3, 1) = point_trans.apply(model.X_base[j].inverse().apply(model.S[j])).block(3,0,3,1);
|
|
}
|
|
|
|
j = model.lambda[j];
|
|
}
|
|
}
|
|
|
|
RBDL_DLLAPI
|
|
void CalcPointJacobian6DPtr (
|
|
Model &model,
|
|
const double *q_ptr,
|
|
unsigned int body_id,
|
|
const Math::Vector3d &point_position,
|
|
double *G_ptr,
|
|
bool update_kinematics
|
|
) {
|
|
LOG << "-------- " << __func__ << " --------" << std::endl;
|
|
|
|
using namespace RigidBodyDynamics::Math;
|
|
|
|
// update the Kinematics if necessary
|
|
if (update_kinematics) {
|
|
UpdateKinematicsCustomPtr (model, q_ptr, NULL, NULL);
|
|
}
|
|
|
|
VectorNdRef Q = VectorFromPtr(const_cast<double*>(q_ptr), model.q_size);
|
|
MatrixNdRef G = MatrixFromPtr(const_cast<double*>(G_ptr), 6, model.qdot_size);
|
|
|
|
SpatialTransform point_trans = SpatialTransform (Matrix3d::Identity(), CalcBodyToBaseCoordinates (model, Q, body_id, point_position, false));
|
|
|
|
assert (G.rows() == 6 && G.cols() == model.qdot_size );
|
|
|
|
unsigned int reference_body_id = body_id;
|
|
|
|
if (model.IsFixedBodyId(body_id)) {
|
|
unsigned int fbody_id = body_id - model.fixed_body_discriminator;
|
|
reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
|
|
}
|
|
|
|
unsigned int j = reference_body_id;
|
|
|
|
while (j != 0) {
|
|
unsigned int q_index = model.mJoints[j].q_index;
|
|
|
|
if (model.mJoints[j].mDoFCount == 3) {
|
|
G.block(0, q_index, 6, 3) = ((point_trans * model.X_base[j].inverse()).toMatrix() * model.multdof3_S[j]).block(0,0,6,3);
|
|
} else {
|
|
G.block(0,q_index, 6, 1) = point_trans.apply(model.X_base[j].inverse().apply(model.S[j])).block(0,0,6,1);
|
|
}
|
|
|
|
j = model.lambda[j];
|
|
}
|
|
}
|
|
|
|
RBDL_DLLAPI
|
|
void CalcBodySpatialJacobianPtr (
|
|
Model &model,
|
|
const double *q_ptr,
|
|
unsigned int body_id,
|
|
double *G_ptr,
|
|
bool update_kinematics
|
|
) {
|
|
LOG << "-------- " << __func__ << " --------" << std::endl;
|
|
|
|
using namespace RigidBodyDynamics::Math;
|
|
|
|
// update the Kinematics if necessary
|
|
if (update_kinematics) {
|
|
UpdateKinematicsCustomPtr (model, q_ptr, NULL, NULL);
|
|
}
|
|
|
|
MatrixNdRef G = MatrixFromPtr(const_cast<double*>(G_ptr), 6, model.qdot_size);
|
|
|
|
assert (G.rows() == 6 && G.cols() == model.qdot_size );
|
|
|
|
unsigned int reference_body_id = body_id;
|
|
|
|
SpatialTransform base_to_body;
|
|
|
|
if (model.IsFixedBodyId(body_id)) {
|
|
unsigned int fbody_id = body_id - model.fixed_body_discriminator;
|
|
reference_body_id = model.mFixedBodies[fbody_id].mMovableParent;
|
|
base_to_body = model.mFixedBodies[fbody_id].mParentTransform * model.X_base[reference_body_id];
|
|
} else {
|
|
base_to_body = model.X_base[reference_body_id];
|
|
}
|
|
|
|
unsigned int j = reference_body_id;
|
|
|
|
while (j != 0) {
|
|
unsigned int q_index = model.mJoints[j].q_index;
|
|
|
|
if (model.mJoints[j].mDoFCount == 3) {
|
|
G.block(0,q_index,6,3) = (base_to_body * model.X_base[j].inverse()).toMatrix() * model.multdof3_S[j];
|
|
} else {
|
|
G.block(0,q_index,6,1) = base_to_body.apply(model.X_base[j].inverse().apply(model.S[j]));
|
|
}
|
|
|
|
j = model.lambda[j];
|
|
}
|
|
}
|
|
|
|
RBDL_DLLAPI
|
|
void InverseDynamicsPtr (
|
|
Model &model,
|
|
const double *q_ptr,
|
|
const double *qdot_ptr,
|
|
const double *qddot_ptr,
|
|
const double *tau_ptr,
|
|
std::vector<Math::SpatialVector> *f_ext
|
|
) {
|
|
LOG << "-------- " << __func__ << " --------" << std::endl;
|
|
|
|
using namespace RigidBodyDynamics::Math;
|
|
|
|
VectorNdRef Q = VectorFromPtr(const_cast<double*>(q_ptr), model.q_size);
|
|
VectorNdRef QDot = VectorFromPtr(const_cast<double*>(qdot_ptr), model.qdot_size);
|
|
VectorNdRef QDDot = VectorFromPtr(const_cast<double*>(qddot_ptr), model.qdot_size);
|
|
VectorNdRef Tau = VectorFromPtr(const_cast<double*>(tau_ptr), model.qdot_size);
|
|
|
|
// Reset the velocity of the root body
|
|
model.v[0].setZero();
|
|
model.a[0].set (0., 0., 0., -model.gravity[0], -model.gravity[1], -model.gravity[2]);
|
|
|
|
for (unsigned int i = 1; i < model.mBodies.size(); i++) {
|
|
unsigned int q_index = model.mJoints[i].q_index;
|
|
unsigned int lambda = model.lambda[i];
|
|
|
|
jcalc (model, i, Q, QDot);
|
|
|
|
if (lambda != 0) {
|
|
model.X_base[i] = model.X_lambda[i] * model.X_base[lambda];
|
|
} else {
|
|
model.X_base[i] = model.X_lambda[i];
|
|
}
|
|
|
|
model.v[i] = model.X_lambda[i].apply(model.v[lambda]) + model.v_J[i];
|
|
model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]);
|
|
|
|
if (model.mJoints[i].mDoFCount == 3) {
|
|
model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i] + model.multdof3_S[i] * Vector3d (QDDot[q_index], QDDot[q_index + 1], QDDot[q_index + 2]);
|
|
} else {
|
|
model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i] + model.S[i] * QDDot[q_index];
|
|
}
|
|
|
|
if (!model.mBodies[i].mIsVirtual) {
|
|
model.f[i] = model.I[i] * model.a[i] + crossf(model.v[i],model.I[i] * model.v[i]);
|
|
} else {
|
|
model.f[i].setZero();
|
|
}
|
|
|
|
if (f_ext != NULL && (*f_ext)[i] != SpatialVector::Zero())
|
|
model.f[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i];
|
|
}
|
|
|
|
for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) {
|
|
if (model.mJoints[i].mDoFCount == 3) {
|
|
Tau.block<3,1>(model.mJoints[i].q_index, 0) = model.multdof3_S[i].transpose() * model.f[i];
|
|
} else {
|
|
Tau[model.mJoints[i].q_index] = model.S[i].dot(model.f[i]);
|
|
}
|
|
|
|
if (model.lambda[i] != 0) {
|
|
model.f[model.lambda[i]] = model.f[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.f[i]);
|
|
}
|
|
}
|
|
}
|
|
|
|
RBDL_DLLAPI
|
|
void NonlinearEffectsPtr (
|
|
Model &model,
|
|
const double *q_ptr,
|
|
const double *qdot_ptr,
|
|
const double *tau_ptr
|
|
) {
|
|
LOG << "-------- " << __func__ << " --------" << std::endl;
|
|
|
|
using namespace RigidBodyDynamics::Math;
|
|
|
|
VectorNdRef Q = VectorFromPtr(const_cast<double*>(q_ptr), model.q_size);
|
|
VectorNdRef QDot = VectorFromPtr(const_cast<double*>(qdot_ptr), model.qdot_size);
|
|
VectorNdRef Tau = VectorFromPtr(const_cast<double*>(tau_ptr), model.qdot_size);
|
|
|
|
SpatialVector spatial_gravity (0., 0., 0., -model.gravity[0], -model.gravity[1], -model.gravity[2]);
|
|
|
|
// Reset the velocity of the root body
|
|
model.v[0].setZero();
|
|
model.a[0] = spatial_gravity;
|
|
|
|
for (unsigned int i = 1; i < model.mJointUpdateOrder.size(); i++) {
|
|
jcalc (model, model.mJointUpdateOrder[i], Q, QDot);
|
|
}
|
|
|
|
for (unsigned int i = 1; i < model.mBodies.size(); i++) {
|
|
if (model.lambda[i] == 0) {
|
|
model.v[i] = model.v_J[i];
|
|
model.a[i] = model.X_lambda[i].apply(spatial_gravity);
|
|
} else {
|
|
model.v[i] = model.X_lambda[i].apply(model.v[model.lambda[i]]) + model.v_J[i];
|
|
model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]);
|
|
model.a[i] = model.X_lambda[i].apply(model.a[model.lambda[i]]) + model.c[i];
|
|
}
|
|
|
|
if (!model.mBodies[i].mIsVirtual) {
|
|
model.f[i] = model.I[i] * model.a[i] + crossf(model.v[i],model.I[i] * model.v[i]);
|
|
} else {
|
|
model.f[i].setZero();
|
|
}
|
|
}
|
|
|
|
for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) {
|
|
if (model.mJoints[i].mDoFCount == 3) {
|
|
Tau.block<3,1>(model.mJoints[i].q_index, 0) = model.multdof3_S[i].transpose() * model.f[i];
|
|
} else {
|
|
Tau[model.mJoints[i].q_index] = model.S[i].dot(model.f[i]);
|
|
}
|
|
|
|
if (model.lambda[i] != 0) {
|
|
model.f[model.lambda[i]] = model.f[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.f[i]);
|
|
}
|
|
}
|
|
}
|
|
|
|
RBDL_DLLAPI
|
|
inline void CompositeRigidBodyAlgorithmPtr (
|
|
Model& model,
|
|
const double *q_ptr,
|
|
double *H_ptr,
|
|
bool update_kinematics = true
|
|
) {
|
|
using namespace RigidBodyDynamics::Math;
|
|
|
|
VectorNdRef&& Q = VectorFromPtr(const_cast<double*>(q_ptr), model.q_size);
|
|
MatrixNdRef&& H = MatrixFromPtr(H_ptr, model.qdot_size, model.qdot_size);
|
|
|
|
assert (H.rows() == model.dof_count && H.cols() == model.dof_count);
|
|
|
|
for (unsigned int i = 1; i < model.mBodies.size(); i++) {
|
|
if (update_kinematics) {
|
|
jcalc_X_lambda_S (model, i, Q);
|
|
}
|
|
model.Ic[i] = model.I[i];
|
|
}
|
|
|
|
for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) {
|
|
if (model.lambda[i] != 0) {
|
|
model.Ic[model.lambda[i]] = model.Ic[model.lambda[i]] + model.X_lambda[i].applyTranspose(model.Ic[i]);
|
|
}
|
|
|
|
unsigned int dof_index_i = model.mJoints[i].q_index;
|
|
|
|
if (model.mJoints[i].mDoFCount == 3) {
|
|
Matrix63 F_63 = model.Ic[i].toMatrix() * model.multdof3_S[i];
|
|
H.block<3,3>(dof_index_i, dof_index_i) = model.multdof3_S[i].transpose() * F_63;
|
|
|
|
unsigned int j = i;
|
|
unsigned int dof_index_j = dof_index_i;
|
|
|
|
while (model.lambda[j] != 0) {
|
|
F_63 = model.X_lambda[j].toMatrixTranspose() * (F_63);
|
|
j = model.lambda[j];
|
|
dof_index_j = model.mJoints[j].q_index;
|
|
|
|
if (model.mJoints[j].mDoFCount == 3) {
|
|
Matrix3d H_temp2 = F_63.transpose() * (model.multdof3_S[j]);
|
|
|
|
H.block<3,3>(dof_index_i,dof_index_j) = H_temp2;
|
|
H.block<3,3>(dof_index_j,dof_index_i) = H_temp2.transpose();
|
|
} else {
|
|
Vector3d H_temp2 = F_63.transpose() * (model.S[j]);
|
|
|
|
H.block<3,1>(dof_index_i,dof_index_j) = H_temp2;
|
|
H.block<1,3>(dof_index_j,dof_index_i) = H_temp2.transpose();
|
|
}
|
|
}
|
|
} else {
|
|
SpatialVector F = model.Ic[i] * model.S[i];
|
|
H(dof_index_i, dof_index_i) = model.S[i].dot(F);
|
|
|
|
unsigned int j = i;
|
|
unsigned int dof_index_j = dof_index_i;
|
|
|
|
while (model.lambda[j] != 0) {
|
|
F = model.X_lambda[j].applyTranspose(F);
|
|
j = model.lambda[j];
|
|
dof_index_j = model.mJoints[j].q_index;
|
|
|
|
if (model.mJoints[j].mDoFCount == 3) {
|
|
Vector3d H_temp2 = (F.transpose() * model.multdof3_S[j]).transpose();
|
|
|
|
LOG << F.transpose() << std::endl << model.multdof3_S[j] << std::endl;
|
|
LOG << H_temp2.transpose() << std::endl;
|
|
|
|
H.block<1,3>(dof_index_i,dof_index_j) = H_temp2.transpose();
|
|
H.block<3,1>(dof_index_j,dof_index_i) = H_temp2;
|
|
} else {
|
|
H(dof_index_i,dof_index_j) = F.dot(model.S[j]);
|
|
H(dof_index_j,dof_index_i) = H(dof_index_i,dof_index_j);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
RBDL_DLLAPI
|
|
void ForwardDynamicsPtr (
|
|
Model &model,
|
|
const double *q_ptr,
|
|
const double *qdot_ptr,
|
|
const double *tau_ptr,
|
|
const double *qddot_ptr,
|
|
std::vector<Math::SpatialVector> *f_ext
|
|
) {
|
|
LOG << "-------- " << __func__ << " --------" << std::endl;
|
|
|
|
using namespace RigidBodyDynamics::Math;
|
|
|
|
VectorNdRef&& Q = VectorFromPtr(const_cast<double*>(q_ptr), model.q_size);
|
|
VectorNdRef&& QDot = VectorFromPtr(const_cast<double*>(qdot_ptr), model.qdot_size);
|
|
VectorNdRef&& QDDot = VectorFromPtr(const_cast<double*>(qddot_ptr), model.qdot_size);
|
|
VectorNdRef&& Tau = VectorFromPtr(const_cast<double*>(tau_ptr), model.qdot_size);
|
|
|
|
SpatialVector spatial_gravity (0., 0., 0., model.gravity[0], model.gravity[1], model.gravity[2]);
|
|
|
|
unsigned int i = 0;
|
|
|
|
LOG << "Q = " << Q.transpose() << std::endl;
|
|
LOG << "QDot = " << QDot.transpose() << std::endl;
|
|
LOG << "Tau = " << Tau.transpose() << std::endl;
|
|
LOG << "---" << std::endl;
|
|
|
|
// Reset the velocity of the root body
|
|
model.v[0].setZero();
|
|
|
|
for (i = 1; i < model.mBodies.size(); i++) {
|
|
unsigned int lambda = model.lambda[i];
|
|
|
|
jcalc (model, i, Q, QDot);
|
|
|
|
if (lambda != 0)
|
|
model.X_base[i] = model.X_lambda[i] * model.X_base[lambda];
|
|
else
|
|
model.X_base[i] = model.X_lambda[i];
|
|
|
|
model.v[i] = model.X_lambda[i].apply( model.v[lambda]) + model.v_J[i];
|
|
|
|
/*
|
|
LOG << "v_J (" << i << "):" << std::endl << v_J << std::endl;
|
|
LOG << "v_lambda" << i << ":" << std::endl << model.v.at(lambda) << std::endl;
|
|
LOG << "X_base (" << i << "):" << std::endl << model.X_base[i] << std::endl;
|
|
LOG << "X_lambda (" << i << "):" << std::endl << model.X_lambda[i] << std::endl;
|
|
LOG << "SpatialVelocity (" << i << "): " << model.v[i] << std::endl;
|
|
*/
|
|
|
|
model.c[i] = model.c_J[i] + crossm(model.v[i],model.v_J[i]);
|
|
model.I[i].setSpatialMatrix (model.IA[i]);
|
|
|
|
model.pA[i] = crossf(model.v[i],model.I[i] * model.v[i]);
|
|
|
|
if (f_ext != NULL && (*f_ext)[i] != SpatialVector::Zero()) {
|
|
LOG << "External force (" << i << ") = " << model.X_base[i].toMatrixAdjoint() * (*f_ext)[i] << std::endl;
|
|
model.pA[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i];
|
|
}
|
|
}
|
|
|
|
// ClearLogOutput();
|
|
|
|
LOG << "--- first loop ---" << std::endl;
|
|
|
|
for (i = model.mBodies.size() - 1; i > 0; i--) {
|
|
unsigned int q_index = model.mJoints[i].q_index;
|
|
|
|
if (model.mJoints[i].mDoFCount == 3) {
|
|
model.multdof3_U[i] = model.IA[i] * model.multdof3_S[i];
|
|
#ifdef EIGEN_CORE_H
|
|
model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse().eval();
|
|
#else
|
|
model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse();
|
|
#endif
|
|
Vector3d tau_temp (Tau[q_index], Tau[q_index + 1], Tau[q_index + 2]);
|
|
|
|
model.multdof3_u[i] = tau_temp - model.multdof3_S[i].transpose() * model.pA[i];
|
|
|
|
// LOG << "multdof3_u[" << i << "] = " << model.multdof3_u[i].transpose() << std::endl;
|
|
unsigned int lambda = model.lambda[i];
|
|
if (lambda != 0) {
|
|
SpatialMatrix Ia = model.IA[i] - model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_U[i].transpose();
|
|
SpatialVector pa = model.pA[i] + Ia * model.c[i] + model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_u[i];
|
|
#ifdef EIGEN_CORE_H
|
|
model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
|
|
model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
|
|
#else
|
|
model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
|
|
model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
|
|
#endif
|
|
LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl;
|
|
}
|
|
} else {
|
|
model.U[i] = model.IA[i] * model.S[i];
|
|
model.d[i] = model.S[i].dot(model.U[i]);
|
|
model.u[i] = Tau[q_index] - model.S[i].dot(model.pA[i]);
|
|
// LOG << "u[" << i << "] = " << model.u[i] << std::endl;
|
|
|
|
unsigned int lambda = model.lambda[i];
|
|
if (lambda != 0) {
|
|
SpatialMatrix Ia = model.IA[i] - model.U[i] * (model.U[i] / model.d[i]).transpose();
|
|
SpatialVector pa = model.pA[i] + Ia * model.c[i] + model.U[i] * model.u[i] / model.d[i];
|
|
#ifdef EIGEN_CORE_H
|
|
model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
|
|
model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
|
|
#else
|
|
model.IA[lambda] += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix();
|
|
model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
|
|
#endif
|
|
LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl;
|
|
}
|
|
}
|
|
}
|
|
|
|
// ClearLogOutput();
|
|
|
|
model.a[0] = spatial_gravity * -1.;
|
|
|
|
for (i = 1; i < model.mBodies.size(); i++) {
|
|
unsigned int q_index = model.mJoints[i].q_index;
|
|
unsigned int lambda = model.lambda[i];
|
|
SpatialTransform X_lambda = model.X_lambda[i];
|
|
|
|
model.a[i] = X_lambda.apply(model.a[lambda]) + model.c[i];
|
|
LOG << "a'[" << i << "] = " << model.a[i].transpose() << std::endl;
|
|
|
|
if (model.mJoints[i].mDoFCount == 3) {
|
|
Vector3d qdd_temp = model.multdof3_Dinv[i] * (model.multdof3_u[i] - model.multdof3_U[i].transpose() * model.a[i]);
|
|
QDDot[q_index] = qdd_temp[0];
|
|
QDDot[q_index + 1] = qdd_temp[1];
|
|
QDDot[q_index + 2] = qdd_temp[2];
|
|
model.a[i] = model.a[i] + model.multdof3_S[i] * qdd_temp;
|
|
} else {
|
|
QDDot[q_index] = (1./model.d[i]) * (model.u[i] - model.U[i].dot(model.a[i]));
|
|
model.a[i] = model.a[i] + model.S[i] * QDDot[q_index];
|
|
}
|
|
}
|
|
|
|
LOG << "QDDot = " << QDDot.transpose() << std::endl;
|
|
}
|
|
|
|
RBDL_DLLAPI
|
|
void ForwardDynamicsConstraintsDirectPtr (
|
|
Model &model,
|
|
const double *q_ptr,
|
|
const double *qdot_ptr,
|
|
const double *tau_ptr,
|
|
ConstraintSet &CS,
|
|
double *qddot_ptr
|
|
) {
|
|
LOG << "-------- " << __func__ << " --------" << std::endl;
|
|
|
|
using namespace RigidBodyDynamics::Math;
|
|
|
|
VectorNdRef&& Q = VectorFromPtr(const_cast<double*>(q_ptr), model.q_size);
|
|
VectorNdRef&& QDot = VectorFromPtr(const_cast<double*>(qdot_ptr), model.qdot_size);
|
|
VectorNdRef&& QDDot = VectorFromPtr(const_cast<double*>(qddot_ptr), model.qdot_size);
|
|
VectorNdRef&& Tau = VectorFromPtr(const_cast<double*>(tau_ptr), model.qdot_size);
|
|
|
|
// create copy of non-const accelerations
|
|
VectorNd QDDot_dummy = QDDot;
|
|
|
|
LOG << "Q = " << Q.transpose() << std::endl;
|
|
LOG << "QDot = " << QDot.transpose() << std::endl;
|
|
LOG << "Tau = " << Tau.transpose() << std::endl;
|
|
|
|
LOG << "QDDot = " << QDDot.transpose() << std::endl;
|
|
LOG << "QDDot_dummy = " << QDDot_dummy.transpose() << std::endl;
|
|
|
|
// calling non-pointer version
|
|
ForwardDynamicsConstraintsDirect (
|
|
model, Q, QDot, Tau, CS, QDDot_dummy
|
|
);
|
|
|
|
for (int i = 0; i < model.qdot_size; ++i) {
|
|
QDDot[i] = QDDot_dummy[i];
|
|
}
|
|
LOG << "QDDot = " << QDDot.transpose() << std::endl;
|
|
LOG << "QDDot_dummy = " << QDDot_dummy.transpose() << std::endl;
|
|
LOG << "---" << std::endl;
|
|
}
|
|
}
|