/// Gets the spatial constraints for this joint
SMatrix6N& Joint::get_spatial_constraints(ReferenceFrameType rftype, SMatrix6N& s)
{
const unsigned X = 0, Y = 1, Z = 2;
Real Cq[7];
// get the outboard link and its orientation quaternion
RigidBodyPtr outboard = get_outboard_link();
assert(outboard);
const Quat& q = outboard->get_orientation();
// resize the spatial constraint matrix
s.resize(6, num_constraint_eqns());
// calculate the constraint Jacobian in relation to the outboard link
for (unsigned i=0; i< num_constraint_eqns(); i++)
{
// calculate the constraint Jacobian
calc_constraint_jacobian_rodrigues(outboard, i, Cq);
// convert the differential quaternion constraints to an angular velocity
// representation
Vector3 omega = q.G_mult(Cq[3], Cq[4], Cq[5], Cq[6]) * (Real) 0.5;
// setup the column of the constraint Jacobian
s(0,i) = omega[X];
s(1,i) = omega[Y];
s(2,i) = omega[Z];
s(3,i) = Cq[X];
s(4,i) = Cq[Y];
s(5,i) = Cq[Z];
}
// TODO: this should be in link-global frame -- fix this!
assert(false);
// convert to link frame if necessary (constraints computed in global frame)
if (rftype == eLink)
{
SpatialTransform Xi0 = outboard->get_spatial_transform_global_to_link();
for (unsigned i=0; i< num_constraint_eqns(); i++)
{
SVector6 scol = s.get_column(i);
scol = Xi0.transform(scol);
s.set_column(i, scol);
}
}
return s;
}
/**
* \param inv_dyn_data a mapping from links to the external forces (and
* torques) applied to them and to the desired inner joint
* accelerations; note that all links in the robot should be included
* in this map (even the base link, although inner joint acceleration
* is not applicable in that case and will be ignored for it)
* \return a mapping from joints to actuator forces
*/
map<JointPtr, VectorN> RNEAlgorithm::calc_inv_dyn_floating_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, I;
vector<SVector6> Z, v, a;
FILE_LOG(LOG_DYNAMICS) << "RNEAlgorithm::calc_inv_dyn_floating_base() entered" << endl;
// get the reference frame type
ReferenceFrameType rftype = body->computation_frame_type;
// get the set of links
const vector<RigidBodyPtr>& links = body->get_links();
// ** STEP 0: compute isolated inertias
// get the isolated inertiae
Iiso.resize(links.size());
for (unsigned i=0; i< links.size(); i++)
{
unsigned idx = links[i]->get_index();
Iiso[idx] = links[i]->get_spatial_iso_inertia(rftype);
}
// ** STEP 1: compute velocities and relative accelerations
// set all desired velocities and accelerations (latter relative to the base)
// to zero initially
v.resize(links.size());
a.resize(links.size());
for (unsigned i=0; i< links.size(); i++)
v[i] = a[i] = ZEROS_6;
// get the base link
RigidBodyPtr base = links.front();
// set velocity for the base
v.front() = base->get_spatial_velocity(rftype);
// 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();
// add all child links of the link to the processing 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 parent link
RigidBodyPtr parent(link->get_parent_link());
// get the index of this link and its parent
unsigned i = link->get_index();
unsigned im1 = parent->get_index();
// get the spatial axes (and derivative) of this link's inner joint
JointPtr joint(link->get_inner_joint_implicit());
const SMatrix6N& s = joint->get_spatial_axes(rftype);
const SMatrix6N& s_dot = joint->get_spatial_axes_dot(rftype);
// compute s * qdot
SVector6 sqd = s.mult(joint->qd);
// get the desired acceleration for the current link
idd_iter = inv_dyn_data.find(link);
assert(idd_iter != inv_dyn_data.end());
const VectorN& qdd_des = idd_iter->second.qdd;
// compute velocity and relative acceleration
v[i] = v[im1] + sqd;
a[i] = a[im1] + s.mult(qdd_des) + s_dot.mult(joint->qd) + SVector6::spatial_cross(v[i], sqd);
FILE_LOG(LOG_DYNAMICS) << " s: " << s << endl;
FILE_LOG(LOG_DYNAMICS) << " velocity for link " << links[i]->id << ": " << v[i] << endl;
FILE_LOG(LOG_DYNAMICS) << " s * qdd: " << s.mult(qdd_des) << endl;
FILE_LOG(LOG_DYNAMICS) << " v x s * qd: " << SVector6::spatial_cross(v[i], sqd) << endl;
FILE_LOG(LOG_DYNAMICS) << " relative accel for link " << links[i]->id << ": " << a[i] << endl;
}
// ** STEP 2: compute composite inertias and Z.A. forces
// resize vectors of spatial inertias and Z.A. vectors
I.resize(links.size());
Z.resize(links.size());
// zero out I and Z
for (unsigned i=0; i< links.size(); i++)
{
I[i].set_zero();
Z[i] = ZEROS_6;
}
// set all spatial isolated inertias and Z.A. forces
for (unsigned i=0; i< links.size(); i++)
{
// get the i'th link
RigidBodyPtr link = links[i];
unsigned idx = link->get_index();
// add the spatial isolated inertia for this link to the composite inertia
I[idx] += Iiso[idx];
// setup forces due to (relative) acceleration on link
Z[idx] = Iiso[idx] * a[idx];
// add coriolis and centrifugal forces on link
Z[idx] += SVector6::spatial_cross(v[i], Iiso[idx] * v[idx]);
// determine external forces on the link 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));
// transform external forces and subtract from Z.A. vector
SpatialTransform X_0_i = link->get_spatial_transform_link_to_global();
Z[idx] -= X_0_i.transform(fx);
FILE_LOG(LOG_DYNAMICS) << " -- processing link " << link->id << endl;
FILE_LOG(LOG_DYNAMICS) << " external force / torque: " << fext << " / " << text << endl;
FILE_LOG(LOG_DYNAMICS) << " ZA vector: " << Z[idx] << endl;
FILE_LOG(LOG_DYNAMICS) << " isolated spatial-inertia: " << endl << Iiso[idx];
}
// *** compute composite inertias and zero acceleration vectors
// setup vector that indicates when links have been processed
vector<bool> processed(links.size(), false);
// put all leaf links into the queue
for (unsigned i=0; i< links.size(); i++)
if (links[i]->num_child_links() == 0)
link_queue.push(links[i]);
// 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();
// get the index for this link
unsigned i = link->get_index();
// see whether this link has already been processed
if (processed[i])
continue;
// process the parent link, if possible
RigidBodyPtr parent(link->get_parent_link());
if (parent)
{
// put the parent on the queue
link_queue.push(parent);
// get the parent index
unsigned im1 = parent->get_index();
// add this inertia and Z.A. force to its parent
I[im1] += I[i];
Z[im1] += Z[i];
// indicate that the link has been processed
processed[i] = true;
}
}
// ** STEP 3: compute base acceleration
a.front() = I.front().inverse_mult(-Z.front());
SpatialTransform X_i_0 = base->get_spatial_transform_global_to_link();
FILE_LOG(LOG_DYNAMICS) << " Composite inertia for the base: " << endl << I.front();
FILE_LOG(LOG_DYNAMICS) << " ZA vector for the base (link frame): " << X_i_0.transform(Z.front()) << endl;
FILE_LOG(LOG_DYNAMICS) << " Determined base acceleration (link frame): " << X_i_0.transform(a.front()) << endl;
// ** STEP 4: compute joint forces
// setup the map of actuator forces
map<JointPtr, VectorN> actuator_forces;
// compute the forces
for (unsigned i=1; i< links.size(); i++)
{
unsigned idx = links[i]->get_index();
JointPtr joint(links[i]->get_inner_joint_implicit());
const SMatrix6N& s = joint->get_spatial_axes(rftype);
VectorN& Q = actuator_forces[joint];
s.transpose_mult((I[idx] * a.front()) + Z[idx], Q);
FILE_LOG(LOG_DYNAMICS) << " processing link: " << links[i]->id << endl;
FILE_LOG(LOG_DYNAMICS) << " spatial axis: " << endl << s;
FILE_LOG(LOG_DYNAMICS) << " I: " << endl << I[idx];
FILE_LOG(LOG_DYNAMICS) << " Z: " << endl << Z[idx];
FILE_LOG(LOG_DYNAMICS) << " actuator force: " << actuator_forces[joint] << endl;
}
FILE_LOG(LOG_DYNAMICS) << "RNEAlgorithm::calc_inv_dyn_floating_base() exited" << endl;
return actuator_forces;
}
