29 #include <boost/format.hpp> 31 template<
class TMatrix>
35 std::cout <<
"Vector " << name << M << std::endl;
37 std::cout <<
"Matrix " << name <<
": \n";
38 for(
size_t i=0;
i < M.size1();
i++){
39 for(
size_t j=0; j < M.cols(); j++){
40 std::cout << boost::format(
" %6.3f ") % M(
i, j);
42 std::cout << std::endl;
47 template<
class TVector>
50 std::cout <<
"Vector " << name << M << std::endl;
76 int numLinks =
body->numLinks();
78 for(
int i=1;
i < numLinks; ++
i){
109 uorg. resize(numLinks);
123 q0. resize(numLinks);
124 dq0.resize(numLinks);
125 dq. resize(numLinks);
126 ddq.resize(numLinks);
170 Matrix33::ColXpr
x = R.col(0);
171 Matrix33::ColXpr
y = R.col(1);
172 Matrix33::ColXpr z = R.col(2);
174 z = x.cross(y).normalized();
183 body->setVirtualJointForces();
209 for(
int i=0;
i <
n; ++
i){
236 for(
int i=0;
i < numHighGainJoints; ++
i){
256 int numLinks =
body->numLinks();
258 for(
int i=1;
i < numLinks; ++
i){
327 root->
w =
w0 + root->
dw * dt;
335 int n =
body->numLinks();
336 for(
int i=1;
i <
n; ++
i){
338 link->
q =
q0 [
i] + dt * link->
dq;
340 dq [
i] += r * link->
dq;
368 Link* root = traverse[0];
372 root->
vo = root->
v - root->
w.cross(root->
p);
375 for(
int i=0;
i <
n; ++
i){
376 Link* link = traverse[
i];
384 link->
p = parent->
R * (link->
b + link->
q * link->
d) + parent->
p;
387 link->
sv.noalias() = parent->
R * link->
d;
393 link->
p = parent->
R * link->
b + parent->
p;
394 link->
sw.noalias() = parent->
R * link->
a;
395 link->
sv = link->
p.cross(link->
sw);
396 link->
w = link->
dq * link->
sw + parent->
w;
401 link->
p = parent->
R * link->
b + parent->
p;
404 link->
vo = parent->
vo;
409 goto COMMON_CALCS_FOR_ALL_JOINT_TYPES;
412 link->
vo = link->
dq * link->
sv + parent->
vo;
414 Vector3 dsv(parent->
w.cross(link->
sv) + parent->
vo.cross(link->
sw));
416 link->
cv = link->
dq * dsv;
417 link->
cw = link->
dq * dsw;
420 COMMON_CALCS_FOR_ALL_JOINT_TYPES:
423 link->
v = link->
vo + link->
w.cross(link->
p);
425 link->
wc = link->
R * link->
c + link->
p;
426 Matrix33 Iw(link->
R * link->
I * link->
R.transpose());
428 link->
Iww = link->
m * (c_hat * c_hat.transpose()) + Iw;
429 link->
Iwv = link->
m * c_hat;
434 link->
pf = link->
w.cross(P);
435 link->
ptau = link->
vo.cross(P) + link->
w.cross(L);
449 int numLinks =
body->numLinks();
452 for(
int i=1;
i < numLinks; ++
i){
468 for(
int i=0;
i < 3; ++
i){
473 for(
int i=0;
i < 3; ++
i){
483 for(
int i=0;
i < 3; ++
i){
488 for(
int i=0;
i < 3; ++
i){
515 for(
int i=0;
i <
M11.cols(); ++
i){
518 for(
int i=0;
i <
M12.cols(); ++
i){
522 for(
int i=1;
i < numLinks; ++
i){
542 tau -= root->
p.cross(f);
543 M.block<6,1>(0, column) << f, tau;
557 link->
dw = parent->
dw + link->
cw + link->
sw * link->
ddq;
561 out_tau = link->
ptau;
571 out_f += link->
m * link->
dvo + link->
Iwv.transpose() * link->
dw;
572 out_tau += link->
Iwv * link->
dvo + link->
Iww * link->
dw;
574 link->
u = link->
sv.dot(out_f) + link->
sw.dot(out_tau);
591 int n =
body->numLinks();
592 for(
int i=0;
i <
n; ++
i){
614 link->
u = link->
sv.dot(out_f) + link->
sw.dot(out_tau);
632 root->
dvo = root->
dv - root->
dw.cross(root->
p) - root->
w.cross(root->
v);
644 for(
unsigned int i=1;
i <
body->numLinks(); ++
i){
657 for(
unsigned int i=1;
i <
body->numLinks(); ++
i){
673 for(
unsigned int i=1;
i <
body->numLinks(); ++
i){
686 for(
unsigned int i=1;
i <
body->numLinks(); ++
i){
690 link->
fext = fextorg;
700 c1.segment(3,3) = tauext;
712 a =
M11.colPivHouseholderQr().solve(
c1);
717 root->
dw = a.segment(3, 3);
735 link->
dw = parent->
dw + link->
cw + link->
sw * link->
ddq;
739 out_tau = link->
ptau;
761 out_f += link->
m * link->
dvo + link->
Iwv.transpose() * link->
dw;
762 out_tau += link->
Iwv * link->
dvo + link->
Iww * link->
dw;
765 link->
u = link->
sv.dot(out_f) + link->
sw.dot(out_tau);
773 sensor->
f = sensorR.transpose() *
f;
774 sensor->
tau = sensorR.transpose() * (-out_tau - sensorPos.cross(f));
784 int n =
body->numLinks();
788 for(
int i=0;
i <
n; ++
i){
796 for(
int i=0;
i < numForceSensors; ++
i){
809 hasSensorsAbove |= (info.
sensor != 0);
The header file of the LinkTraverse class.
Vector3 ptau
bias force (torque element)
Matrix33 Iwv
bottom left block (transpose of top right block) of the articulated inertia
Vector3 pf
bias force (linear element)
virtual void initializeSensors()
Vector3 fext
external force
Vector3 vo
translation elements of spacial velocity
void calcd1(Link *link, Vector3 &out_f, Vector3 &out_tau)
std::vector< double > dq0
double ddq
joint acceleration
png_infop png_charpp name
translational joint (1 dof)
static const bool debugMode
enum hrp::ForwardDynamics::@2 integrationMode
void solveUnknownAccels()
Vector3 w
angular velocity, omega
static void putVector(TVector &M, char *name)
void calcMotionWithEulerMethod()
std::vector< ForceSensorInfo > forceSensorInfo
boost::shared_ptr< Body > BodyPtr
ForwardDynamicsMM(BodyPtr body)
std::vector< Link * > highGainModeJoints
static void SE3exp(Vector3 &out_p, Matrix33 &out_R, const Vector3 &p0, const Matrix33 &R0, const Vector3 &w, const Vector3 &vo, double dt)
update position/orientation using spatial velocity
void calcAccelFKandForceSensorValues()
void calcInverseDynamics(Link *link, Vector3 &out_f, Vector3 &out_tau)
Vector3 tauext
external torque (around the world origin)
Vector3 dv
linear acceleration
Matrix33 rodrigues(const Vector3 &axis, double q)
void integrateRungeKuttaOneStep(double r, double dt)
static const bool CALC_ALL_JOINT_TORQUES
Vector3 b
relative position (parent local)
static const bool ROOT_ATT_NORMALIZATION_ENABLED
Vector3 dvo
derivative of vo
void updateForceSensorInfo(Link *link, bool hasSensorsAbove)
Vector3 dw
derivative of omega
Vector3 cw
dsw * dq (cross velocity term)
bool accelSolverInitialized
std::vector< double > ddq
virtual void updateSensorsFinal()
virtual void initializeSensors()
static void putMatrix(TMatrix &M, char *name)
Vector3 cv
dsv * dq (cross velocity term)
Matrix33 I
inertia tensor (self local, around c)
Matrix33 hat(const Vector3 &c)
void initializeAccelSolver()
void setColumnOfMassMatrix(dmatrix &M, int column)
std::vector< Link * > torqueModeJoints
virtual void calcNextState()
Matrix33 Iww
bottm right block of the articulated inertia
Vector3 a
rotational joint axis (self local)
Vector3 d
translation joint axis (self local)
virtual void initialize()
void preserveHighGainModeJointState()
Vector3 c
center of mass (self local)
bool isNoUnknownAccelMode
unsigned int numLinks() const
double Jm2
Equivalent rotor inertia: n^2*Jm [kg.m^2].
void calcPositionAndVelocityFK()
void calcMotionWithRungeKuttaMethod()