/**
* Computed joint actuator forces are stored in inv_dyn_data.
*/
map<JointPtr, VectorN> RNEAlgorithm::calc_inv_dyn_fixed_base(RCArticulatedBodyPtr body, const map<RigidBodyPtr, RCArticulatedBodyInvDynData>& inv_dyn_data) const
{
queue<RigidBodyPtr> link_queue;
map<RigidBodyPtr, RCArticulatedBodyInvDynData>::const_iterator idd_iter;
vector<SpatialRBInertia> Iiso;
FILE_LOG(LOG_DYNAMICS) << "RNEAlgorithm::calc_inv_dyn_fixed_base() entered" << endl;
// get the reference frame for computation
ReferenceFrameType rftype = body->computation_frame_type;
// ** STEP 0: compute isolated inertias
// get the set of links
const vector<RigidBodyPtr>& links = body->get_links();
// get the isolated inertiae
Iiso.resize(links.size());
for (unsigned i=1; i< links.size(); i++)
{
unsigned idx = links[i]->get_index();
Iiso[idx] = links[i]->get_spatial_iso_inertia(rftype);
}
// ** STEP 1: compute velocities and accelerations
// get the base link
RigidBodyPtr base = links.front();
// setup the vector of link accelerations
vector<SVector6> accels(links.size(), ZEROS_6);
// add all child links of the base to the processing queue
list<RigidBodyPtr> child_links;
base->get_child_links(std::back_inserter(child_links));
BOOST_FOREACH(RigidBodyPtr rb, child_links)
link_queue.push(rb);
// process all links
while (!link_queue.empty())
{
// get the link off of the front of the queue
RigidBodyPtr link = link_queue.front();
link_queue.pop();
unsigned idx = link->get_index();
// push all children of the link onto the queue
child_links.clear();
link->get_child_links(std::back_inserter(child_links));
BOOST_FOREACH(RigidBodyPtr rb, child_links)
link_queue.push(rb);
// get the link's parent
RigidBodyPtr parent(link->get_parent_link());
unsigned pidx = parent->get_index();
// get the joint for this link
JointPtr joint(link->get_inner_joint_implicit());
// get the spatial link velocity
const SVector6& v = link->get_spatial_velocity(rftype);
// get the reference to the spatial link acceleration
SVector6& a = accels[idx];
// get spatial axes for this link's inner joint
const SMatrix6N& s = joint->get_spatial_axes(rftype);
// get derivative of the spatial axes for this link's inner joint
const SMatrix6N& s_dot = joint->get_spatial_axes_dot(rftype);
// get the current joint velocity
const VectorN& qd = joint->qd;
// **** compute acceleration
// get the desired joint acceleration
idd_iter = inv_dyn_data.find(link);
assert(idd_iter != inv_dyn_data.end());
const VectorN& qdd_des = idd_iter->second.qdd;
// add this link's contribution
a += SVector6::spatial_cross(v, s.mult(qd)) + s.mult(qdd_des) + s_dot.mult(qd);
// now add parent's contribution
if (rftype == eGlobal)
a += accels[pidx];
else
{
SpatialTransform X_i_im1 = link->get_spatial_transform_forward();
a += X_i_im1.transform(accels[pidx]);
}
FILE_LOG(LOG_DYNAMICS) << " computing link velocity / acceleration; processing link " << link->id << endl;
FILE_LOG(LOG_DYNAMICS) << " spatial axis: " << s << endl;
FILE_LOG(LOG_DYNAMICS) << " spatial joint velocity: " << s.mult(qd) << endl;
FILE_LOG(LOG_DYNAMICS) << " link velocity: " << v << endl;
FILE_LOG(LOG_DYNAMICS) << " link accel: " << a << endl;
}
// ** STEP 2: compute link forces -- backward recursion
vector<bool> processed(links.size(), false);
vector<SVector6> forces(links.size(), SVector6(0,0,0,0,0,0));
// add all leaf links to the queue
for (unsigned i=1; i< links.size(); i++)
if (links[i]->num_child_links() == 0)
link_queue.push(links[i]);
// process all links up to, but not including, the base
while (!link_queue.empty())
{
// get the link off of the front of the queue
RigidBodyPtr link = link_queue.front();
link_queue.pop();
unsigned idx = link->get_index();
// if this link has already been processed, do not process it again
if (processed[idx])
continue;
// if the link is the base, continue the loop
if (link->is_base())
continue;
// link is not the base; add the parent to the queue for processing
RigidBodyPtr parent(link->get_parent_link());
link_queue.push(parent);
unsigned pidx = parent->get_index();
// get the forces for this link and this link's parent
SVector6& fi = forces[idx];
SVector6& fim1 = forces[pidx];
FILE_LOG(LOG_DYNAMICS) << " computing necessary force; processing link " << link->id << endl;
FILE_LOG(LOG_DYNAMICS) << " currently determined link force: " << fi << endl;
FILE_LOG(LOG_DYNAMICS) << " I * a = " << (Iiso[idx] * accels[idx]) << endl;
// get the spatial velocity for this link
const SVector6& v = link->get_spatial_velocity(rftype);
// add I*a to the link force
fi += Iiso[idx] * accels[idx];
// update the determined force to this link w/Coriolis + centrifugal terms
fi += SVector6::spatial_cross(v, Iiso[idx] * v);
FILE_LOG(LOG_DYNAMICS) << " force (+ I*a): " << fi << endl;
// determine external forces in link frame
idd_iter = inv_dyn_data.find(link);
assert(idd_iter != inv_dyn_data.end());
const Vector3& fext = idd_iter->second.fext;
const Vector3& text = idd_iter->second.text;
const Matrix4& T = link->get_transform();
SVector6 fx(T.transpose_mult_vector(fext), T.transpose_mult_vector(text));
// subtract external forces in the appropriate frame
if (rftype == eGlobal)
{
SpatialTransform X_0_i = link->get_spatial_transform_link_to_global();
fi -= X_0_i.transform(fx);
}
else
fi -= fx;
FILE_LOG(LOG_DYNAMICS) << " force on link after subtracting external force: " << fi << endl;
// indicate that this link has been processed
processed[idx] = true;
// update the parent force, if the parent is not the base
if (parent->is_base())
continue;
else
if (rftype == eGlobal)
fim1 += fi;
else
fim1 += link->get_spatial_transform_backward().transform(fi);
}
// ** STEP 3: compute joint forces
// setup a map from joints to actuator forces
map<JointPtr, VectorN> actuator_forces;
// compute actuator forces
for (unsigned i=1; i< links.size(); i++)
{
RigidBodyPtr link = links[i];
JointPtr joint(link->get_inner_joint_implicit());
const SMatrix6N& s = joint->get_spatial_axes(rftype);
VectorN& Q = actuator_forces[joint];
s.transpose_mult(forces[link->get_index()], Q);
FILE_LOG(LOG_DYNAMICS) << "joint " << joint->id << " inner joint force: " << actuator_forces[joint] << endl;
}
FILE_LOG(LOG_DYNAMICS) << "RNEAlgorithm::calc_inv_dyn_fixed_base() exited" << endl;
return actuator_forces;
}
/**
* \pre Uses joint accelerations computed by forward dynamics, so the
* appropriate forward dynamics function must be run first.
*/
void RNEAlgorithm::calc_constraint_forces(RCArticulatedBodyPtr body)
{
queue<RigidBodyPtr> link_queue;
SMatrix6N s;
vector<SpatialRBInertia> Iiso;
FILE_LOG(LOG_DYNAMICS) << "RNEAlgorithm::calc_constraint_forces() entered" << endl;
// get the reference frame for computation
ReferenceFrameType rftype = body->computation_frame_type;
// ** STEP 0: compute isolated inertias
// get the set of links
const vector<RigidBodyPtr>& links = body->get_links();
// get the isolated inertiae
Iiso.resize(links.size());
for (unsigned i=1; i< links.size(); i++)
{
unsigned idx = links[i]->get_index();
Iiso[idx] = links[i]->get_spatial_iso_inertia(rftype);
}
// ** STEP 1: compute velocities and accelerations
// get the base link
RigidBodyPtr base = links.front();
// setup the vector of link accelerations
vector<SVector6> accels(links.size(), ZEROS_6);
// add all child links of the base to the processing queue
list<RigidBodyPtr> child_links;
base->get_child_links(std::back_inserter(child_links));
BOOST_FOREACH(RigidBodyPtr rb, child_links)
link_queue.push(rb);
// ** STEP 1: compute link forces -- backward recursion
vector<bool> processed(links.size(), false);
vector<SVector6> forces(links.size(), ZEROS_6);
// add all leaf links to the queue
for (unsigned i=1; i< links.size(); i++)
if (links[i]->num_child_links() == 0)
link_queue.push(links[i]);
// process all links up to, but not including, the base
while (!link_queue.empty())
{
// get the link off of the front of the queue and push all children of the link onto the queue
RigidBodyPtr link = link_queue.front();
link_queue.pop();
unsigned idx = link->get_index();
// if this link has already been processed, do not process it again
if (processed[idx])
continue;
// if the link is the base, continue the loop
if (link->is_base())
continue;
// link is not the base; add the parent to the queue for processing
RigidBodyPtr parent(link->get_parent_link());
link_queue.push(parent);
unsigned pidx = parent->get_index();
// get the forces for this link and this link's parent
SVector6& fi = forces[idx];
SVector6& fim1 = forces[pidx];
// get this link's acceleration
SVector6 a = link->get_spatial_accel(rftype);
FILE_LOG(LOG_DYNAMICS) << " computing necessary force; processing link " << link->id << endl;
FILE_LOG(LOG_DYNAMICS) << " currently determined link force: " << fi << endl;
FILE_LOG(LOG_DYNAMICS) << " I * a = " << (Iiso[idx] * a) << endl;
// get the spatial velocity for this link
const SVector6& v = link->get_spatial_velocity(rftype);
// add I*a to the link force
fi += Iiso[idx] * a;
// update the determined force to this link w/Coriolis + centrifugal terms
fi += SVector6::spatial_cross(v, Iiso[idx] * v);
FILE_LOG(LOG_DYNAMICS) << " force (+ I*a): " << fi << endl;
// determine external forces in link frame
const Vector3& fext = link->sum_forces();
const Vector3& text = link->sum_torques();
const Matrix4& T = link->get_transform();
SVector6 fx(T.transpose_mult_vector(fext), T.transpose_mult_vector(text));
// subtract external forces in the appropriate frame
if (rftype == eGlobal)
{
SpatialTransform X_0_i = link->get_spatial_transform_link_to_global();
fi -= X_0_i.transform(fx);
}
else
fi -= fx;
FILE_LOG(LOG_DYNAMICS) << " force on link after subtracting external force: " << fi << endl;
// indicate that this link has been processed
processed[idx] = true;
// update the parent force, if the parent is not the base
if (parent->is_base())
continue;
else
{
if (rftype == eGlobal)
fim1 += fi;
else
fim1 += link->get_spatial_transform_backward().transform(fi);
}
}
// ** STEP 2: compute constraint forces
// compute actuator forces
for (unsigned i=1; i< links.size(); i++)
{
RigidBodyPtr link = links[i];
JointPtr joint(link->get_inner_joint_implicit());
joint->get_spatial_constraints(rftype, s);
s.transpose_mult(forces[link->get_index()], joint->lambda);
FILE_LOG(LOG_DYNAMICS) << "joint " << joint->id << " constraint force: " << joint->lambda << endl;
}
FILE_LOG(LOG_DYNAMICS) << "RNEAlgorithm::calc_constraint_forces() exited" << endl;
}
