martin
/
rbdlsim
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
rbdlsim/3rdparty/rbdl/src/Dynamics.cc

863 lines
30 KiB
C++
Raw Normal View History

Initial commit
2020-10-03 22:55:14 +02:00
/*
* RBDL - Rigid Body Dynamics Library
* Copyright (c) 2011-2018 Martin Felis <martin@fysx.org>
*
* Licensed under the zlib license. See LICENSE for more details.
*/
#include <iostream>
#include <limits>
#include <cassert>
#include <cstring>
#include "rbdl/rbdl_mathutils.h"
#include "rbdl/Logging.h"
#include "rbdl/Model.h"
#include "rbdl/Joint.h"
#include "rbdl/Body.h"
#include "rbdl/Dynamics.h"
#include "rbdl/Kinematics.h"
namespace RigidBodyDynamics {
using namespace Math;
RBDL_DLLAPI void InverseDynamics (
Model &model,
const VectorNd &Q,
const VectorNd &QDot,
const VectorNd &QDDot,
VectorNd &Tau,
std::vector<SpatialVector> *f_ext) {
LOG << "-------- " << __func__ << " --------" << std::endl;
// 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);
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].mJointType != JointTypeCustom){
if (model.mJoints[i].mDoFCount == 1) {
model.a[i] = model.X_lambda[i].apply(model.a[lambda])
+ model.c[i]
+ model.S[i] * QDDot[q_index];
} else 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 if(model.mJoints[i].mJointType == JointTypeCustom){
unsigned int k = model.mJoints[i].custom_joint_index;
VectorNd customJointQDDot(model.mCustomJoints[k]->mDoFCount);
for(unsigned z = 0; z < model.mCustomJoints[k]->mDoFCount; ++z){
customJointQDDot[z] = QDDot[q_index+z];
}
model.a[i] = model.X_lambda[i].apply(model.a[lambda])
+ model.c[i]
+ model.mCustomJoints[k]->S * customJointQDDot;
}
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) {
for (unsigned int i = 1; i < model.mBodies.size(); i++) {
unsigned int lambda = model.lambda[i];
model.X_base[i] = model.X_lambda[i] * model.X_base[lambda];
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].mJointType != JointTypeCustom){
if (model.mJoints[i].mDoFCount == 1) {
Tau[model.mJoints[i].q_index] = model.S[i].dot(model.f[i]);
} else 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 if (model.mJoints[i].mJointType == JointTypeCustom) {
unsigned int k = model.mJoints[i].custom_joint_index;
Tau.block(model.mJoints[i].q_index,0,
model.mCustomJoints[k]->mDoFCount, 1)
= model.mCustomJoints[k]->S.transpose() * 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 NonlinearEffects (
Model &model,
const VectorNd &Q,
const VectorNd &QDot,
VectorNd &Tau,
std::vector<Math::SpatialVector> *f_ext) {
LOG << "-------- " << __func__ << " --------" << std::endl;
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]);
if (f_ext != NULL && (*f_ext)[i] != SpatialVector::Zero()) {
model.f[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i];
}
} else {
model.f[i].setZero();
}
}
for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) {
if(model.mJoints[i].mJointType != JointTypeCustom){
if (model.mJoints[i].mDoFCount == 1) {
Tau[model.mJoints[i].q_index]
= model.S[i].dot(model.f[i]);
} else 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 if(model.mJoints[i].mJointType == JointTypeCustom) {
unsigned int k = model.mJoints[i].custom_joint_index;
Tau.block(model.mJoints[i].q_index,0,
model.mCustomJoints[k]->mDoFCount, 1)
= model.mCustomJoints[k]->S.transpose() * 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 CompositeRigidBodyAlgorithm (
Model& model,
const VectorNd &Q,
MatrixNd &H,
bool update_kinematics) {
LOG << "-------- " << __func__ << " --------" << std::endl;
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 == 1
&& model.mJoints[i].mJointType != JointTypeCustom) {
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].mJointType != JointTypeCustom) {
if (model.mJoints[j].mDoFCount == 1) {
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);
} else 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 if (model.mJoints[j].mJointType == JointTypeCustom){
unsigned int k = model.mJoints[j].custom_joint_index;
unsigned int dof = model.mCustomJoints[k]->mDoFCount;
VectorNd H_temp2 =
(F.transpose() * model.mCustomJoints[k]->S).transpose();
LOG << F.transpose()
<< std::endl
<< model.mCustomJoints[j]->S << std::endl;
LOG << H_temp2.transpose() << std::endl;
H.block(dof_index_i,dof_index_j,1,dof) = H_temp2.transpose();
H.block(dof_index_j,dof_index_i,dof,1) = H_temp2;
}
}
} else if (model.mJoints[i].mDoFCount == 3
&& model.mJoints[i].mJointType != JointTypeCustom) {
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].mJointType != JointTypeCustom){
if (model.mJoints[j].mDoFCount == 1) {
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 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 if (model.mJoints[j].mJointType == JointTypeCustom){
unsigned int k = model.mJoints[j].custom_joint_index;
unsigned int dof = model.mCustomJoints[k]->mDoFCount;
MatrixNd H_temp2 = F_63.transpose() * (model.mCustomJoints[k]->S);
H.block(dof_index_i,dof_index_j,3,dof) = H_temp2;
H.block(dof_index_j,dof_index_i,dof,3) = H_temp2.transpose();
}
}
} else if (model.mJoints[i].mJointType == JointTypeCustom) {
unsigned int kI = model.mJoints[i].custom_joint_index;
unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
MatrixNd F_Nd = model.Ic[i].toMatrix()
* model.mCustomJoints[kI]->S;
H.block(dof_index_i, dof_index_i,dofI,dofI)
= model.mCustomJoints[kI]->S.transpose() * F_Nd;
unsigned int j = i;
unsigned int dof_index_j = dof_index_i;
while (model.lambda[j] != 0) {
F_Nd = model.X_lambda[j].toMatrixTranspose() * (F_Nd);
j = model.lambda[j];
dof_index_j = model.mJoints[j].q_index;
if(model.mJoints[j].mJointType != JointTypeCustom){
if (model.mJoints[j].mDoFCount == 1) {
MatrixNd H_temp2 = F_Nd.transpose() * (model.S[j]);
H.block( dof_index_i, dof_index_j,
H_temp2.rows(),H_temp2.cols()) = H_temp2;
H.block(dof_index_j,dof_index_i,
H_temp2.cols(),H_temp2.rows()) = H_temp2.transpose();
} else if (model.mJoints[j].mDoFCount == 3) {
MatrixNd H_temp2 = F_Nd.transpose() * (model.multdof3_S[j]);
H.block(dof_index_i, dof_index_j,
H_temp2.rows(),H_temp2.cols()) = H_temp2;
H.block(dof_index_j, dof_index_i,
H_temp2.cols(),H_temp2.rows()) = H_temp2.transpose();
}
} else if (model.mJoints[j].mJointType == JointTypeCustom){
unsigned int k = model.mJoints[j].custom_joint_index;
unsigned int dof = model.mCustomJoints[k]->mDoFCount;
MatrixNd H_temp2 = F_Nd.transpose() * (model.mCustomJoints[k]->S);
H.block(dof_index_i,dof_index_j,3,dof) = H_temp2;
H.block(dof_index_j,dof_index_i,dof,3) = H_temp2.transpose();
}
}
}
}
}
RBDL_DLLAPI void ForwardDynamics (
Model &model,
const VectorNd &Q,
const VectorNd &QDot,
const VectorNd &Tau,
VectorNd &QDDot,
std::vector<SpatialVector> *f_ext) {
LOG << "-------- " << __func__ << " --------" << std::endl;
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 << "X_J (" << i << "):" << std::endl << X_J << std::endl;
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 == 1
&& model.mJoints[i].mJointType != JointTypeCustom) {
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;
}
} else if (model.mJoints[i].mDoFCount == 3
&& model.mJoints[i].mJointType != JointTypeCustom) {
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.block(q_index,0,3,1));
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 if (model.mJoints[i].mJointType == JointTypeCustom) {
unsigned int kI = model.mJoints[i].custom_joint_index;
unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
model.mCustomJoints[kI]->U =
model.IA[i] * model.mCustomJoints[kI]->S;
#ifdef EIGEN_CORE_H
model.mCustomJoints[kI]->Dinv
= (model.mCustomJoints[kI]->S.transpose()
* model.mCustomJoints[kI]->U).inverse().eval();
#else
model.mCustomJoints[kI]->Dinv
= (model.mCustomJoints[kI]->S.transpose()
* model.mCustomJoints[kI]->U).inverse();
#endif
VectorNd tau_temp(Tau.block(q_index,0,dofI,1));
model.mCustomJoints[kI]->u = tau_temp
- model.mCustomJoints[kI]->S.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.mCustomJoints[kI]->U
* model.mCustomJoints[kI]->Dinv
* model.mCustomJoints[kI]->U.transpose());
SpatialVector pa = model.pA[i]
+ Ia * model.c[i]
+ (model.mCustomJoints[kI]->U
* model.mCustomJoints[kI]->Dinv
* model.mCustomJoints[kI]->u);
#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 == 1
&& model.mJoints[i].mJointType != JointTypeCustom) {
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];
} else if (model.mJoints[i].mDoFCount == 3
&& model.mJoints[i].mJointType != JointTypeCustom) {
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 if (model.mJoints[i].mJointType == JointTypeCustom) {
unsigned int kI = model.mJoints[i].custom_joint_index;
unsigned int dofI=model.mCustomJoints[kI]->mDoFCount;
VectorNd qdd_temp = model.mCustomJoints[kI]->Dinv
* ( model.mCustomJoints[kI]->u
- model.mCustomJoints[kI]->U.transpose()
* model.a[i]);
for(unsigned int z=0; z < dofI; ++z){
QDDot[q_index+z] = qdd_temp[z];
}
model.a[i] = model.a[i]
+ model.mCustomJoints[kI]->S * qdd_temp;
}
}
LOG << "QDDot = " << QDDot.transpose() << std::endl;
}
RBDL_DLLAPI void ForwardDynamicsLagrangian (
Model &model,
const VectorNd &Q,
const VectorNd &QDot,
const VectorNd &Tau,
VectorNd &QDDot,
Math::LinearSolver linear_solver,
std::vector<SpatialVector> *f_ext,
Math::MatrixNd *H,
Math::VectorNd *C) {
LOG << "-------- " << __func__ << " --------" << std::endl;
bool free_H = false;
bool free_C = false;
if (H == NULL) {
H = new MatrixNd (MatrixNd::Zero(model.dof_count, model.dof_count));
free_H = true;
}
if (C == NULL) {
C = new VectorNd (VectorNd::Zero(model.dof_count));
free_C = true;
}
// we set QDDot to zero to compute C properly with the InverseDynamics
// method.
QDDot.setZero();
InverseDynamics (model, Q, QDot, QDDot, (*C), f_ext);
CompositeRigidBodyAlgorithm (model, Q, *H, false);
LOG << "A = " << std::endl << *H << std::endl;
LOG << "b = " << std::endl << *C * -1. + Tau << std::endl;
switch (linear_solver) {
case (LinearSolverPartialPivLU) :
QDDot = H->partialPivLu().solve (*C * -1. + Tau);
break;
case (LinearSolverColPivHouseholderQR) :
QDDot = H->colPivHouseholderQr().solve (*C * -1. + Tau);
break;
case (LinearSolverHouseholderQR) :
QDDot = H->householderQr().solve (*C * -1. + Tau);
break;
case (LinearSolverLLT) :
QDDot = H->llt().solve (*C * -1. + Tau);
break;
default:
LOG << "Error: Invalid linear solver: " << linear_solver << std::endl;
assert (0);
break;
}
if (free_C) {
delete C;
}
if (free_H) {
delete H;
}
LOG << "x = " << QDDot << std::endl;
}
RBDL_DLLAPI void CalcMInvTimesTau ( Model &model,
const VectorNd &Q,
const VectorNd &Tau,
VectorNd &QDDot,
bool update_kinematics) {
LOG << "Q = " << Q.transpose() << std::endl;
LOG << "---" << std::endl;
// Reset the velocity of the root body
model.v[0].setZero();
model.a[0].setZero();
if (update_kinematics) {
for (unsigned int i = 1; i < model.mBodies.size(); i++) {
jcalc_X_lambda_S (model, model.mJointUpdateOrder[i], Q);
model.v_J[i].setZero();
model.v[i].setZero();
model.c[i].setZero();
model.pA[i].setZero();
model.I[i].setSpatialMatrix (model.IA[i]);
}
}
for (unsigned int i = 1; i < model.mBodies.size(); i++) {
model.pA[i].setZero();
}
// ClearLogOutput();
if (update_kinematics) {
// Compute Articulate Body Inertias
for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) {
// unsigned int q_index = model.mJoints[i].q_index;
if (model.mJoints[i].mDoFCount == 1
&& model.mJoints[i].mJointType != JointTypeCustom) {
model.U[i] = model.IA[i] * model.S[i];
model.d[i] = model.S[i].dot(model.U[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();
#ifdef EIGEN_CORE_H
model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose()
* Ia
* model.X_lambda[i].toMatrix();
#else
model.IA[lambda] += model.X_lambda[i].toMatrixTranspose()
* Ia
* model.X_lambda[i].toMatrix();
#endif
}
} else if (model.mJoints[i].mDoFCount == 3
&& model.mJoints[i].mJointType != JointTypeCustom) {
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
// LOG << "mCustomJoints[kI]->u[" << i << "] = "
//<< model.mCustomJoints[kI]->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());
#ifdef EIGEN_CORE_H
model.IA[lambda].noalias() +=
model.X_lambda[i].toMatrixTranspose()
* Ia
* model.X_lambda[i].toMatrix();
#else
model.IA[lambda] +=
model.X_lambda[i].toMatrixTranspose()
* Ia * model.X_lambda[i].toMatrix();
#endif
}
} else if (model.mJoints[i].mJointType == JointTypeCustom) {
unsigned int kI = model.mJoints[i].custom_joint_index;
// unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
model.mCustomJoints[kI]->U = model.IA[i] * model.mCustomJoints[kI]->S;
#ifdef EIGEN_CORE_H
model.mCustomJoints[kI]->Dinv = (model.mCustomJoints[kI]->S.transpose()
* model.mCustomJoints[kI]->U
).inverse().eval();
#else
model.mCustomJoints[kI]->Dinv=(model.mCustomJoints[kI]->S.transpose()
* model.mCustomJoints[kI]->U
).inverse();
#endif
// LOG << "mCustomJoints[kI]->u[" << i << "] = "
//<< model.mCustomJoints[kI]->u.transpose() << std::endl;
unsigned int lambda = model.lambda[i];
if (lambda != 0) {
SpatialMatrix Ia = model.IA[i]
- ( model.mCustomJoints[kI]->U
* model.mCustomJoints[kI]->Dinv
* model.mCustomJoints[kI]->U.transpose());
#ifdef EIGEN_CORE_H
model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose()
* Ia
* model.X_lambda[i].toMatrix();
#else
model.IA[lambda] += model.X_lambda[i].toMatrixTranspose()
* Ia * model.X_lambda[i].toMatrix();
#endif
}
}
}
}
// compute articulated bias forces
for (unsigned int i = model.mBodies.size() - 1; i > 0; i--) {
unsigned int q_index = model.mJoints[i].q_index;
if (model.mJoints[i].mDoFCount == 1
&& model.mJoints[i].mJointType != JointTypeCustom) {
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) {
SpatialVector pa = model.pA[i] + model.U[i] * model.u[i] / model.d[i];
#ifdef EIGEN_CORE_H
model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa);
#else
model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
#endif
LOG << "pA[" << lambda << "] = "
<< model.pA[lambda].transpose() << std::endl;
}
} else if (model.mJoints[i].mDoFCount == 3
&& model.mJoints[i].mJointType != JointTypeCustom) {
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) {
SpatialVector pa = model.pA[i]
+ model.multdof3_U[i]
* model.multdof3_Dinv[i]
* model.multdof3_u[i];
#ifdef EIGEN_CORE_H
model.pA[lambda].noalias() +=
model.X_lambda[i].applyTranspose(pa);
#else
model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
#endif
LOG << "pA[" << lambda << "] = "
<< model.pA[lambda].transpose() << std::endl;
}
} else if (model.mJoints[i].mJointType == JointTypeCustom) {
unsigned int kI = model.mJoints[i].custom_joint_index;
unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
VectorNd tau_temp(Tau.block(q_index,0,dofI,1));
model.mCustomJoints[kI]->u =
tau_temp - ( model.mCustomJoints[kI]->S.transpose()* model.pA[i]);
// LOG << "mCustomJoints[kI]->u"
// << model.mCustomJoints[kI]->u.transpose() << std::endl;
unsigned int lambda = model.lambda[i];
if (lambda != 0) {
SpatialVector pa = model.pA[i]
+ ( model.mCustomJoints[kI]->U
* model.mCustomJoints[kI]->Dinv
* model.mCustomJoints[kI]->u);
#ifdef EIGEN_CORE_H
model.pA[lambda].noalias() +=
model.X_lambda[i].applyTranspose(pa);
#else
model.pA[lambda] += model.X_lambda[i].applyTranspose(pa);
#endif
LOG << "pA[" << lambda << "] = "
<< model.pA[lambda].transpose() << std::endl;
}
}
}
// ClearLogOutput();
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];
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 == 1
&& model.mJoints[i].mJointType != JointTypeCustom) {
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];
} else if (model.mJoints[i].mDoFCount == 3
&& model.mJoints[i].mJointType != JointTypeCustom) {
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 if (model.mJoints[i].mJointType == JointTypeCustom) {
unsigned int kI = model.mJoints[i].custom_joint_index;
unsigned int dofI = model.mCustomJoints[kI]->mDoFCount;
VectorNd qdd_temp = model.mCustomJoints[kI]->Dinv
* ( model.mCustomJoints[kI]->u
- model.mCustomJoints[kI]->U.transpose() * model.a[i]);
for(unsigned z = 0; z < dofI; ++z){
QDDot[q_index+z] = qdd_temp[z];
}
model.a[i] = model.a[i]
+ model.mCustomJoints[kI]->S * qdd_temp;
}
}
LOG << "QDDot = " << QDDot.transpose() << std::endl;
}
} /* namespace RigidBodyDynamics */