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; }
/// 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; }