// write the actuator state to the actuator buffer
void write_state( void ) {
//if( VERBOSE ) printf( "(dynamics::write_state)\n" );
// get a reference to the actuator
JointPtr pivot = pendulum->find_joint( "pivot" );
// write the actuator state out to the command buffer
actuator_msg_c msg = actuator_msg_c( );
msg.header.type = ACTUATOR_MSG_REPLY;
msg.state.position = pivot->q[0];
msg.state.velocity = pivot->qd[0];
msg.state.time = sim->current_time;
// this was a part of the Dynamic ode_both(.) procedure but if the accumulators are
// cleared there they can never change from zero as they were also originally added there.
// So, had to relocate the reset procedure here.
pendulum->reset_accumulators();
// Note: will block on acquiring mutex
if( dyn_amsgbuffer.write( msg ) != BUFFER_ERROR_NONE ) {
sprintf( strbuffer, "(dynamics.cpp) write_state() failed calling actuator_msg_buffer_c.write(msg)\n" );
dyn_error_log.write( strbuffer );
// TODO : return error condition
}
}
void controller(shared_ptr<ControlledBody> cb, double, void*)
{
RCArticulatedBodyPtr rcab = dynamic_pointer_cast<RCArticulatedBody>(cb);
for (unsigned i=0; i< rcab->get_links().size(); i++)
std::cout << "link " << rcab->get_links()[i]->body_id << " pose: " << Pose3d::calc_relative_pose(rcab->get_links()[i]->get_pose(), GLOBAL) << std::endl;
std::cout << "---" << std::endl;
}
/**
* Computed joint actuator forces are stored in inv_dyn_data.
*/
map<JointPtr, VectorN> RNEAlgorithm::calc_inv_dyn(RCArticulatedBodyPtr body, const map<RigidBodyPtr, RCArticulatedBodyInvDynData>& inv_dyn_data)
{
if (!body->is_floating_base())
return calc_inv_dyn_fixed_base(body, inv_dyn_data);
else
return calc_inv_dyn_floating_base(body, inv_dyn_data);
}
/**
publish the state of the actuators to the command buffers. In this case, one actuator
and one controller.
*/
void publish_state( void ) {
//printf( "(dynamics::publish_state)\n" );
// get a reference to the actuator
JointPtr pivot = pendulum->find_joint( "pivot" );
// write the actuator state out to the command buffer
Command cmd = Command( );
cmd.state.position = pivot->q[0];
cmd.state.velocity = pivot->qd[0];
cmd.state.time = sim->current_time;
//cmd.print();
cmdbuffer.write( cmd );
// this was a part of the Dynamic ode_both(.) procedure but if the accumulators are
// cleared there they can never change from zero. So, had to relocate the reset procedure
// here.
pendulum->reset_accumulators();
}
/**
publish the state of the actuators to the command buffers. In this case, one actuator
and one controller.
*/
void publish_state( void ) {
if( VERBOSE ) printf( "(dynamics::publish_state)\n" );
// get a reference to the actuator
JointPtr pivot = pendulum->find_joint( "pivot" );
// write the actuator state out to the command buffer
ActuatorMessage msg = ActuatorMessage( );
msg.header.type = MSG_TYPE_STATE_REPLY;
msg.state.position = pivot->q[0];
msg.state.velocity = pivot->qd[0];
msg.state.time = sim->current_time;
// this was a part of the Dynamic ode_both(.) procedure but if the accumulators are
// cleared there they can never change from zero as they were also originally added there.
// So, had to relocate the reset procedure here.
pendulum->reset_accumulators();
// Note: will block on acquiring mutex
amsgbuffer.write( msg );
}
/**
maybe a misnomer. Externalized, it means to get the command from the command buffer and
act in response (apply a torque). Not necessary to expose this anymore as an external
interface
*/
void get_command( void ) {
//printf( "(dynamics::get_command) " );
// get the command from the controller
Command cmd = cmdbuffer.read( );
//cmd.print();
// apply any force determined by the controller to the actuator
JointPtr pivot = pendulum->find_joint( "pivot" );
VectorN tau( 1 );
tau[0] = cmd.state.torque;
pivot->add_force( tau );
}
/**
maybe a misnomer. Externalized, it means to get the command from the command buffer and
act in response (apply a torque). Not necessary to expose this anymore as an external
interface
*/
void get_command( void ) {
if( VERBOSE ) printf( "(dynamics::get_command)\n" );
// get the command from the controller
ActuatorMessage msg = amsgbuffer.read( );
// Note: will block on acquiring mutex
//printf( "(dynamics::get_command) " );
//msg.print();
// apply any force determined by the controller to the actuator
JointPtr pivot = pendulum->find_joint( "pivot" );
VectorN tau( 1 );
tau[0] = msg.command.torque;
pivot->add_force( tau );
}
/// Controls the pendulum
void control_PD( RCArticulatedBodyPtr pendulum, Real time ) {
const Real Kp = 1.0;
const Real Kv = 0.1;
JointPtr pivot = pendulum->find_joint( "pivot" );
if( !pivot )
std::cerr << "Could not find pivot joint\n";
Real measured_position = pivot->q[0];
Real measured_velocity = pivot->qd[0];
Real desired_position = position_trajectory( time, 0.5, 0.0, PI );
Real desired_velocity = velocity_trajectory( time, 0.5, 0.0, PI );
Real torque = Kp * ( desired_position - measured_position ) + Kv * ( desired_velocity - measured_velocity );
VectorN tau( 1 );
tau[0] = torque;
pivot->add_force( tau );
}
// Read the command ( torque ) from the message buffer.
void read_command( void ) {
actuator_msg_c msg;
//if( VERBOSE ) printf( "(dynamics::read_command)\n" );
// get the command from the controller
// Note: will block on acquiring mutex
if( dyn_amsgbuffer.read( msg ) != BUFFER_ERROR_NONE ) {
sprintf( strbuffer, "(dynamics.cpp) read_command() failed calling actuator_msg_buffer_c.read(msg)\n" );
dyn_error_log.write( strbuffer );
// TODO : return error condition
}
//If( VERBOSE ) msg.print();
// apply any force determined by the controller to the actuator
JointPtr pivot = pendulum->find_joint( "pivot" );
Ravelin::VectorNd tau( 1 );
tau[0] = msg.command.torque;
pivot->add_force( tau );
}
/// Controls the robot
void control_PID(RCArticulatedBodyPtr robot, Real time)
{
Real ACCEL = 10000.0;
// determine joint positions for two dof of shoulder
const Real SPEED = 100;
const Real MAG = 0.25;
Real lsh1 = -1.30 + std::sin(time*SPEED)*MAG;
Real lsh2 = std::sin(2*time*SPEED)*MAG;
Real lsh1_qd = std::cos(time*SPEED)*MAG*SPEED;
Real lsh2_qd = std::cos(2*time*SPEED)*SPEED*MAG*2;
Real lsh1_qdd = -std::sin(time*SPEED)*SPEED*SPEED*MAG;
Real lsh2_qdd = -std::sin(2*SPEED*time)*SPEED*SPEED*MAG*4;
// setup desired joint positions
std::map<std::string, Real> q_des;
// q_des["left-shoulder1-joint"] = -0.79;
// q_des["left-shoulder2-joint"] = 0.0;
q_des["left-shoulder1-joint"] = lsh1;
q_des["left-shoulder2-joint"] = lsh2;
//q_des["left-shoulder1-joint"] = 1.30;
//q_des["left-shoulder2-joint"] = 0.0;
// q_des["left-shoulder1-joint"] = 1.40;
// q_des["left-shoulder2-joint"] = 0.0;
q_des["left-bicep-joint"] = 0.0;
q_des["left-elbow-joint"] = 0.0;
q_des["left-forearm-joint"] = 0.0;
q_des["left-hand-joint"] = 0.0;
q_des["left-claw-left-joint"] = -0.25;
q_des["left-claw-right-joint"] = 0.25;
//q_des["left-claw-left-joint"] = -0.37;
//q_des["left-claw-right-joint"] = 0.37;
/*
q_des["left-shoulder1-joint"] = lsh1;
q_des["left-shoulder2-joint"] = lsh2;
q_des["left-bicep-joint"] = 0.0;
q_des["left-elbow-joint"] = -0.63;
q_des["left-forearm-joint"] = 0.0;
q_des["left-hand-joint"] = 0.0;
q_des["left-claw-left-joint"] = -.198998;
q_des["left-claw-right-joint"] = .199998;
*/
// setup desired joint velocities
std::map<std::string, Real> qd_des;
qd_des["left-shoulder1-joint"] = lsh1_qd;
qd_des["left-shoulder2-joint"] = lsh2_qd;
// qd_des["left-shoulder1-joint"] = 0.0;
// qd_des["left-shoulder2-joint"] = 0.0;
qd_des["left-bicep-joint"] = 0.0;
qd_des["left-elbow-joint"] = 0.0;
qd_des["left-forearm-joint"] = 0.0;
qd_des["left-hand-joint"] = 0.0;
qd_des["left-claw-left-joint"] = 0.0;
qd_des["left-claw-right-joint"] = 0.0;
/*
qd_des["left-shoulder1-joint"] = lsh1_qd;
qd_des["left-shoulder2-joint"] = lsh2_qd;
qd_des["left-bicep-joint"] = 0.0;
qd_des["left-elbow-joint"] = 0.0;
qd_des["left-forearm-joint"] = 0.0;
qd_des["left-hand-joint"] = 0.0;
qd_des["left-claw-left-joint"] = 0.0;
qd_des["left-claw-right-joint"] = 0.0;
*/
// setup gains
std::map<std::string, std::pair<Real, Real> > gains;
gains["left-shoulder1-joint"] = std::make_pair(300,120);
gains["left-shoulder2-joint"] = std::make_pair(300,120);
gains["left-bicep-joint"] = std::make_pair(100,40);
gains["left-elbow-joint"] = std::make_pair(60,24);
gains["left-forearm-joint"] = std::make_pair(25,10);
gains["left-hand-joint"] = std::make_pair(15,6);
gains["left-claw-left-joint"] = std::make_pair(15,6);
gains["left-claw-right-joint"] = std::make_pair(15,6);
/*
gains["left-shoulder1-joint"] = std::make_pair(300,120);
gains["left-shoulder2-joint"] = std::make_pair(300,120);
gains["left-bicep-joint"] = std::make_pair(100,40);
gains["left-elbow-joint"] = std::make_pair(60,24);
gains["left-forearm-joint"] = std::make_pair(25,10);
gains["left-hand-joint"] = std::make_pair(15,6);
gains["left-claw-left-joint"] = std::make_pair(15,6);
gains["left-claw-right-joint"] = std::make_pair(15,6);
*/
// compute inverse dynamics
std::map<RigidBodyPtr, RCArticulatedBodyInvDynData> inv_dyn_data;
for (unsigned i=1; i< robot->get_links().size(); i++)
{
// setup the inverse dynamics data
RCArticulatedBodyInvDynData id_data;
id_data.fext = robot->get_links()[i]->sum_forces();
id_data.text = robot->get_links()[i]->sum_torques();
JointPtr joint(robot->get_links()[i]->get_inner_joint_implicit());
id_data.qdd = VectorN(1);
if (joint->id == "left-claw-right-joint")
id_data.qdd[0] = -ACCEL;
else if (joint->id == "left-claw-left-joint")
id_data.qdd[0] = ACCEL;
else if (joint->id == "left-shoulder1-joint")
id_data.qdd[0] = -ACCEL;
//id_data.qdd[0] = lsh1_qdd;
/*
else if (joint->id == "left-shoulder2-joint")
//id_data.qdd[0] = lsh2_qdd;
*/
else
id_data.qdd[0] = 0;
inv_dyn_data[robot->get_links()[i]] = id_data;
}
// compute inverse dynamics
RNEAlgorithm rne;
std::map<JointPtr, VectorN> actuator_forces = rne.calc_inv_dyn(robot, inv_dyn_data);
// clear and set motor torques
for (std::map<JointPtr, VectorN>::const_iterator i = actuator_forces.begin(); i != actuator_forces.end(); i++)
{
// reset motor torque
i->first->reset_force();
// add computed torque
i->first->add_force(i->second);
// get the two gains
const Real KP = gains[i->first->id].first;
const Real KV = gains[i->first->id].second;
// for outputting desired position and velocity
std::string fname1 = i->first->id + ".pos";
std::string fname2 = i->first->id + ".vel";
std::ofstream out(fname1.c_str(), std::ofstream::app);
out << q_des[i->first->id] << " " << i->first->q[0] << std::endl;
out.close();
out.open(fname2.c_str(), std::ostream::app);
out << qd_des[i->first->id] << " " << i->first->qd[0] << std::endl;
out.close();
// add feedback torque to joints
Real perr = q_des[i->first->id] - i->first->q[0];
Real derr = qd_des[i->first->id] - i->first->qd[0];
VectorN fb_torque(1);
fb_torque[0] = perr*KP + derr*KV;
i->first->add_force(fb_torque);
}
}
/**
* \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;
}
/**
* 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;
}
