|
Blender
V2.59
|
00001 00004 /* $Id: Armature.cpp 35155 2011-02-25 11:45:16Z jesterking $ 00005 * Armature.cpp 00006 * 00007 * Created on: Feb 3, 2009 00008 * Author: benoitbolsee 00009 */ 00010 00011 #include "Armature.hpp" 00012 #include <algorithm> 00013 #include <string.h> 00014 #include <stdlib.h> 00015 00016 namespace iTaSC { 00017 00018 // a joint constraint is characterized by 5 values: tolerance, K, alpha, yd, yddot 00019 static const unsigned int constraintCacheSize = 5; 00020 std::string Armature::m_root = "root"; 00021 00022 Armature::Armature(): 00023 ControlledObject(), 00024 m_tree(), 00025 m_njoint(0), 00026 m_nconstraint(0), 00027 m_noutput(0), 00028 m_neffector(0), 00029 m_finalized(false), 00030 m_cache(NULL), 00031 m_buf(NULL), 00032 m_qCCh(-1), 00033 m_qCTs(0), 00034 m_yCCh(-1), 00035 m_yCTs(0), 00036 m_qKdl(), 00037 m_oldqKdl(), 00038 m_newqKdl(), 00039 m_qdotKdl(), 00040 m_jac(NULL), 00041 m_armlength(0.0), 00042 m_jacsolver(NULL), 00043 m_fksolver(NULL) 00044 { 00045 } 00046 00047 Armature::~Armature() 00048 { 00049 if (m_jac) 00050 delete m_jac; 00051 if (m_jacsolver) 00052 delete m_jacsolver; 00053 if (m_fksolver) 00054 delete m_fksolver; 00055 for (JointConstraintList::iterator it=m_constraints.begin(); it != m_constraints.end(); it++) { 00056 if (*it != NULL) 00057 delete (*it); 00058 } 00059 if (m_buf) 00060 delete [] m_buf; 00061 m_constraints.clear(); 00062 } 00063 00064 Armature::JointConstraint_struct::JointConstraint_struct(SegmentMap::const_iterator _segment, unsigned int _y_nr, ConstraintCallback _function, void* _param, bool _freeParam, bool _substep): 00065 segment(_segment), value(), values(), function(_function), y_nr(_y_nr), param(_param), freeParam(_freeParam), substep(_substep) 00066 { 00067 memset(values, 0, sizeof(values)); 00068 memset(value, 0, sizeof(value)); 00069 values[0].feedback = 20.0; 00070 values[1].feedback = 20.0; 00071 values[2].feedback = 20.0; 00072 values[0].tolerance = 1.0; 00073 values[1].tolerance = 1.0; 00074 values[2].tolerance = 1.0; 00075 values[0].values = &value[0]; 00076 values[1].values = &value[1]; 00077 values[2].values = &value[2]; 00078 values[0].number = 1; 00079 values[1].number = 1; 00080 values[2].number = 1; 00081 switch (segment->second.segment.getJoint().getType()) { 00082 case Joint::RotX: 00083 value[0].id = ID_JOINT_RX; 00084 values[0].id = ID_JOINT_RX; 00085 v_nr = 1; 00086 break; 00087 case Joint::RotY: 00088 value[0].id = ID_JOINT_RY; 00089 values[0].id = ID_JOINT_RY; 00090 v_nr = 1; 00091 break; 00092 case Joint::RotZ: 00093 value[0].id = ID_JOINT_RZ; 00094 values[0].id = ID_JOINT_RZ; 00095 v_nr = 1; 00096 break; 00097 case Joint::TransX: 00098 value[0].id = ID_JOINT_TX; 00099 values[0].id = ID_JOINT_TX; 00100 v_nr = 1; 00101 break; 00102 case Joint::TransY: 00103 value[0].id = ID_JOINT_TY; 00104 values[0].id = ID_JOINT_TY; 00105 v_nr = 1; 00106 break; 00107 case Joint::TransZ: 00108 value[0].id = ID_JOINT_TZ; 00109 values[0].id = ID_JOINT_TZ; 00110 v_nr = 1; 00111 break; 00112 case Joint::Sphere: 00113 values[0].id = value[0].id = ID_JOINT_RX; 00114 values[1].id = value[1].id = ID_JOINT_RY; 00115 values[2].id = value[2].id = ID_JOINT_RZ; 00116 v_nr = 3; 00117 break; 00118 case Joint::Swing: 00119 values[0].id = value[0].id = ID_JOINT_RX; 00120 values[1].id = value[1].id = ID_JOINT_RZ; 00121 v_nr = 2; 00122 break; 00123 case Joint::None: 00124 break; 00125 } 00126 } 00127 00128 Armature::JointConstraint_struct::~JointConstraint_struct() 00129 { 00130 if (freeParam && param) 00131 free(param); 00132 } 00133 00134 void Armature::initCache(Cache *_cache) 00135 { 00136 m_cache = _cache; 00137 m_qCCh = -1; 00138 m_yCCh = -1; 00139 m_buf = NULL; 00140 if (m_cache) { 00141 // add a special channel for the joint 00142 m_qCCh = m_cache->addChannel(this, "q", m_qKdl.rows()*sizeof(double)); 00143 #if 0 00144 // for the constraints, instead of creating many different channels, we will 00145 // create a single channel for all the constraints 00146 if (m_nconstraint) { 00147 m_yCCh = m_cache->addChannel(this, "y", m_nconstraint*constraintCacheSize*sizeof(double)); 00148 m_buf = new double[m_nconstraint*constraintCacheSize]; 00149 } 00150 // store the initial cache position at timestamp 0 00151 pushConstraints(0); 00152 #endif 00153 pushQ(0); 00154 } 00155 } 00156 00157 void Armature::pushQ(CacheTS timestamp) 00158 { 00159 if (m_qCCh >= 0) { 00160 // try to keep the cache if the joints are the same 00161 m_cache->addCacheVectorIfDifferent(this, m_qCCh, timestamp, &m_qKdl(0), m_qKdl.rows(), KDL::epsilon); 00162 m_qCTs = timestamp; 00163 } 00164 } 00165 00166 /* return true if a m_cache position was loaded */ 00167 bool Armature::popQ(CacheTS timestamp) 00168 { 00169 if (m_qCCh >= 0) { 00170 double* item; 00171 item = (double*)m_cache->getPreviousCacheItem(this, m_qCCh, ×tamp); 00172 if (item && m_qCTs != timestamp) { 00173 double& q = m_qKdl(0); 00174 memcpy(&q, item, m_qKdl.rows()*sizeof(q)); 00175 m_qCTs = timestamp; 00176 // changing the joint => recompute the jacobian 00177 updateJacobian(); 00178 } 00179 return (item) ? true : false; 00180 } 00181 return true; 00182 } 00183 #if 0 00184 void Armature::pushConstraints(CacheTS timestamp) 00185 { 00186 if (m_yCCh >= 0) { 00187 double *buf = NULL; 00188 if (m_nconstraint) { 00189 double *item = m_buf; 00190 for (unsigned int i=0; i<m_nconstraint; i++) { 00191 JointConstraint_struct* pConstraint = m_constraints[i]; 00192 *item++ = pConstraint->values.feedback; 00193 *item++ = pConstraint->values.tolerance; 00194 *item++ = pConstraint->value.yd; 00195 *item++ = pConstraint->value.yddot; 00196 *item++ = pConstraint->values.alpha; 00197 } 00198 } 00199 m_cache->addCacheVectorIfDifferent(this, m_yCCh, timestamp, m_buf, m_nconstraint*constraintCacheSize, KDL::epsilon); 00200 m_yCTs = timestamp; 00201 } 00202 } 00203 00204 /* return true if a cache position was loaded */ 00205 bool Armature::popConstraints(CacheTS timestamp) 00206 { 00207 if (m_yCCh >= 0) { 00208 double *item = (double*)m_cache->getPreviousCacheItem(this, m_yCCh, ×tamp); 00209 if (item && m_yCTs != timestamp) { 00210 for (unsigned int i=0; i<m_nconstraint; i++) { 00211 JointConstraint_struct* pConstraint = m_constraints[i]; 00212 if (pConstraint->function != Joint1DOFLimitCallback) { 00213 pConstraint->values.feedback = *item++; 00214 pConstraint->values.tolerance = *item++; 00215 pConstraint->value.yd = *item++; 00216 pConstraint->value.yddot = *item++; 00217 pConstraint->values.alpha = *item++; 00218 } else { 00219 item += constraintCacheSize; 00220 } 00221 } 00222 m_yCTs = timestamp; 00223 } 00224 return (item) ? true : false; 00225 } 00226 return true; 00227 } 00228 #endif 00229 00230 bool Armature::addSegment(const std::string& segment_name, const std::string& hook_name, const Joint& joint, const double& q_rest, const Frame& f_tip, const Inertia& M) 00231 { 00232 if (m_finalized) 00233 return false; 00234 00235 Segment segment(joint, f_tip, M); 00236 if (!m_tree.addSegment(segment, segment_name, hook_name)) 00237 return false; 00238 int ndof = joint.getNDof(); 00239 for (int dof=0; dof<ndof; dof++) { 00240 Joint_struct js(joint.getType(), ndof, (&q_rest)[dof]); 00241 m_joints.push_back(js); 00242 } 00243 m_njoint+=ndof; 00244 return true; 00245 } 00246 00247 bool Armature::getSegment(const std::string& name, const unsigned int q_size, const Joint* &p_joint, double &q_rest, double &q, const Frame* &p_tip) 00248 { 00249 SegmentMap::const_iterator sit = m_tree.getSegment(name); 00250 if (sit == m_tree.getSegments().end()) 00251 return false; 00252 p_joint = &sit->second.segment.getJoint(); 00253 if (q_size < p_joint->getNDof()) 00254 return false; 00255 p_tip = &sit->second.segment.getFrameToTip(); 00256 for (unsigned int dof=0; dof<p_joint->getNDof(); dof++) { 00257 (&q_rest)[dof] = m_joints[sit->second.q_nr+dof].rest; 00258 (&q)[dof] = m_qKdl(sit->second.q_nr+dof); 00259 } 00260 return true; 00261 } 00262 00263 double Armature::getMaxJointChange() 00264 { 00265 if (!m_finalized) 00266 return 0.0; 00267 double maxJoint = 0.0; 00268 for (unsigned int i=0; i<m_njoint; i++) { 00269 // this is a very rough calculation, it doesn't work well for spherical joint 00270 double joint = fabs(m_oldqKdl(i)-m_qKdl(i)); 00271 if (maxJoint < joint) 00272 maxJoint = joint; 00273 } 00274 return maxJoint; 00275 } 00276 00277 double Armature::getMaxEndEffectorChange() 00278 { 00279 if (!m_finalized) 00280 return 0.0; 00281 double maxDelta = 0.0; 00282 double delta; 00283 Twist twist; 00284 for (unsigned int i = 0; i<m_neffector; i++) { 00285 twist = diff(m_effectors[i].pose, m_effectors[i].oldpose); 00286 delta = twist.rot.Norm(); 00287 if (delta > maxDelta) 00288 maxDelta = delta; 00289 delta = twist.vel.Norm(); 00290 if (delta > maxDelta) 00291 maxDelta = delta; 00292 } 00293 return maxDelta; 00294 } 00295 00296 int Armature::addConstraint(const std::string& segment_name, ConstraintCallback _function, void* _param, bool _freeParam, bool _substep) 00297 { 00298 SegmentMap::const_iterator segment_it = m_tree.getSegment(segment_name); 00299 // not suitable for NDof joints 00300 if (segment_it == m_tree.getSegments().end()) { 00301 if (_freeParam && _param) 00302 free(_param); 00303 return -1; 00304 } 00305 JointConstraintList::iterator constraint_it; 00306 JointConstraint_struct* pConstraint; 00307 int iConstraint; 00308 for (iConstraint=0, constraint_it=m_constraints.begin(); constraint_it != m_constraints.end(); constraint_it++, iConstraint++) { 00309 pConstraint = *constraint_it; 00310 if (pConstraint->segment == segment_it) { 00311 // redefining a constraint 00312 if (pConstraint->freeParam && pConstraint->param) { 00313 free(pConstraint->param); 00314 } 00315 pConstraint->function = _function; 00316 pConstraint->param = _param; 00317 pConstraint->freeParam = _freeParam; 00318 pConstraint->substep = _substep; 00319 return iConstraint; 00320 } 00321 } 00322 if (m_finalized) { 00323 if (_freeParam && _param) 00324 free(_param); 00325 return -1; 00326 } 00327 // new constraint, append 00328 pConstraint = new JointConstraint_struct(segment_it, m_noutput, _function, _param, _freeParam, _substep); 00329 m_constraints.push_back(pConstraint); 00330 m_noutput += pConstraint->v_nr; 00331 return m_nconstraint++; 00332 } 00333 00334 int Armature::addLimitConstraint(const std::string& segment_name, unsigned int dof, double _min, double _max) 00335 { 00336 SegmentMap::const_iterator segment_it = m_tree.getSegment(segment_name); 00337 if (segment_it == m_tree.getSegments().end()) 00338 return -1; 00339 const Joint& joint = segment_it->second.segment.getJoint(); 00340 if (joint.getNDof() != 1 && joint.getType() != Joint::Swing) { 00341 // not suitable for Sphere joints 00342 return -1; 00343 } 00344 if ((joint.getNDof() == 1 && dof > 0) || (joint.getNDof() == 2 && dof > 1)) 00345 return -1; 00346 Joint_struct& p_joint = m_joints[segment_it->second.q_nr+dof]; 00347 p_joint.min = _min; 00348 p_joint.max = _max; 00349 p_joint.useLimit = true; 00350 return 0; 00351 } 00352 00353 int Armature::addEndEffector(const std::string& name) 00354 { 00355 const SegmentMap& segments = m_tree.getSegments(); 00356 if (segments.find(name) == segments.end()) 00357 return -1; 00358 00359 EffectorList::const_iterator it; 00360 int ee; 00361 for (it=m_effectors.begin(), ee=0; it!=m_effectors.end(); it++, ee++) { 00362 if (it->name == name) 00363 return ee; 00364 } 00365 if (m_finalized) 00366 return -1; 00367 Effector_struct effector(name); 00368 m_effectors.push_back(effector); 00369 return m_neffector++; 00370 } 00371 00372 void Armature::finalize() 00373 { 00374 unsigned int i, j, c; 00375 if (m_finalized) 00376 return; 00377 initialize(m_njoint, m_noutput, m_neffector); 00378 for (i=c=0; i<m_nconstraint; i++) { 00379 JointConstraint_struct* pConstraint = m_constraints[i]; 00380 for (j=0; j<pConstraint->v_nr; j++, c++) { 00381 m_Cq(c,pConstraint->segment->second.q_nr+j) = 1.0; 00382 m_Wy(c) = pConstraint->values[j].alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/; 00383 } 00384 } 00385 m_jacsolver= new KDL::TreeJntToJacSolver(m_tree); 00386 m_fksolver = new KDL::TreeFkSolverPos_recursive(m_tree); 00387 m_jac = new Jacobian(m_njoint); 00388 m_qKdl.resize(m_njoint); 00389 m_oldqKdl.resize(m_njoint); 00390 m_newqKdl.resize(m_njoint); 00391 m_qdotKdl.resize(m_njoint); 00392 for (i=0; i<m_njoint; i++) { 00393 m_newqKdl(i) = m_oldqKdl(i) = m_qKdl(i) = m_joints[i].rest; 00394 } 00395 updateJacobian(); 00396 // estimate the maximum size of the robot arms 00397 double length; 00398 m_armlength = 0.0; 00399 for (i=0; i<m_neffector; i++) { 00400 length = 0.0; 00401 KDL::SegmentMap::const_iterator sit = m_tree.getSegment(m_effectors[i].name); 00402 while (sit->first != "root") { 00403 Frame tip = sit->second.segment.pose(m_qKdl(sit->second.q_nr)); 00404 length += tip.p.Norm(); 00405 sit = sit->second.parent; 00406 } 00407 if (length > m_armlength) 00408 m_armlength = length; 00409 } 00410 if (m_armlength < KDL::epsilon) 00411 m_armlength = KDL::epsilon; 00412 m_finalized = true; 00413 } 00414 00415 void Armature::pushCache(const Timestamp& timestamp) 00416 { 00417 if (!timestamp.substep && timestamp.cache) { 00418 pushQ(timestamp.cacheTimestamp); 00419 //pushConstraints(timestamp.cacheTimestamp); 00420 } 00421 } 00422 00423 bool Armature::setJointArray(const KDL::JntArray& joints) 00424 { 00425 if (!m_finalized) 00426 return false; 00427 if (joints.rows() != m_qKdl.rows()) 00428 return false; 00429 m_qKdl = joints; 00430 updateJacobian(); 00431 return true; 00432 } 00433 00434 const KDL::JntArray& Armature::getJointArray() 00435 { 00436 return m_qKdl; 00437 } 00438 00439 bool Armature::updateJoint(const Timestamp& timestamp, JointLockCallback& callback) 00440 { 00441 if (!m_finalized) 00442 return false; 00443 00444 // integration and joint limit 00445 // for spherical joint we must use a more sophisticated method 00446 unsigned int q_nr; 00447 double* qdot=&m_qdotKdl(0); 00448 double* q=&m_qKdl(0); 00449 double* newq=&m_newqKdl(0); 00450 double norm, qx, qz, CX, CZ, sx, sz; 00451 bool locked = false; 00452 int unlocked = 0; 00453 00454 for (q_nr=0; q_nr<m_nq; ++q_nr) 00455 m_qdotKdl(q_nr)=m_qdot(q_nr); 00456 00457 for (q_nr=0; q_nr<m_nq; ) { 00458 Joint_struct* joint = &m_joints[q_nr]; 00459 if (!joint->locked) { 00460 switch (joint->type) { 00461 case KDL::Joint::Swing: 00462 { 00463 KDL::Rotation base = KDL::Rot(KDL::Vector(q[0],0.0,q[1])); 00464 (base*KDL::Rot(KDL::Vector(qdot[0],0.0,qdot[1])*timestamp.realTimestep)).GetXZRot().GetValue(newq); 00465 if (joint[0].useLimit) { 00466 if (joint[1].useLimit) { 00467 // elliptical limit 00468 sx = sz = 1.0; 00469 qx = newq[0]; 00470 qz = newq[1]; 00471 // determine in which quadrant we are 00472 if (qx > 0.0 && qz > 0.0) { 00473 CX = joint[0].max; 00474 CZ = joint[1].max; 00475 } else if (qx <= 0.0 && qz > 0.0) { 00476 CX = -joint[0].min; 00477 CZ = joint[1].max; 00478 qx = -qx; 00479 sx = -1.0; 00480 } else if (qx <= 0.0 && qz <= 0.0) { 00481 CX = -joint[0].min; 00482 CZ = -joint[1].min; 00483 qx = -qx; 00484 qz = -qz; 00485 sx = sz = -1.0; 00486 } else { 00487 CX = joint[0].max; 00488 CZ = -joint[0].min; 00489 qz = -qz; 00490 sz = -1.0; 00491 } 00492 if (CX < KDL::epsilon || CZ < KDL::epsilon) { 00493 // quadrant is degenerated 00494 if (qx > CX) { 00495 newq[0] = CX*sx; 00496 joint[0].locked = true; 00497 } 00498 if (qz > CZ) { 00499 newq[1] = CZ*sz; 00500 joint[0].locked = true; 00501 } 00502 } else { 00503 // general case 00504 qx /= CX; 00505 qz /= CZ; 00506 norm = KDL::sqrt(KDL::sqr(qx)+KDL::sqr(qz)); 00507 if (norm > 1.0) { 00508 norm = 1.0/norm; 00509 newq[0] = qx*norm*CX*sx; 00510 newq[1] = qz*norm*CZ*sz; 00511 joint[0].locked = true; 00512 } 00513 } 00514 } else { 00515 // limit on X only 00516 qx = newq[0]; 00517 if (qx > joint[0].max) { 00518 newq[0] = joint[0].max; 00519 joint[0].locked = true; 00520 } else if (qx < joint[0].min) { 00521 newq[0] = joint[0].min; 00522 joint[0].locked = true; 00523 } 00524 } 00525 } else if (joint[1].useLimit) { 00526 // limit on Z only 00527 qz = newq[1]; 00528 if (qz > joint[1].max) { 00529 newq[1] = joint[1].max; 00530 joint[0].locked = true; 00531 } else if (qz < joint[1].min) { 00532 newq[1] = joint[1].min; 00533 joint[0].locked = true; 00534 } 00535 } 00536 if (joint[0].locked) { 00537 // check the difference from previous position 00538 locked = true; 00539 norm = KDL::sqr(newq[0]-q[0])+KDL::sqr(newq[1]-q[1]); 00540 if (norm < KDL::epsilon2) { 00541 // joint didn't move, no need to update the jacobian 00542 callback.lockJoint(q_nr, 2); 00543 } else { 00544 // joint moved, compute the corresponding velocity 00545 double deltaq[2]; 00546 (base.Inverse()*KDL::Rot(KDL::Vector(newq[0],0.0,newq[1]))).GetXZRot().GetValue(deltaq); 00547 deltaq[0] /= timestamp.realTimestep; 00548 deltaq[1] /= timestamp.realTimestep; 00549 callback.lockJoint(q_nr, 2, deltaq); 00550 // no need to update the other joints, it will be done after next rerun 00551 goto end_loop; 00552 } 00553 } else 00554 unlocked++; 00555 break; 00556 } 00557 case KDL::Joint::Sphere: 00558 { 00559 (KDL::Rot(KDL::Vector(q))*KDL::Rot(KDL::Vector(qdot)*timestamp.realTimestep)).GetRot().GetValue(newq); 00560 // no limit on this joint 00561 unlocked++; 00562 break; 00563 } 00564 default: 00565 for (unsigned int i=0; i<joint->ndof; i++) { 00566 newq[i] = q[i]+qdot[i]*timestamp.realTimestep; 00567 if (joint[i].useLimit) { 00568 if (newq[i] > joint[i].max) { 00569 newq[i] = joint[i].max; 00570 joint[0].locked = true; 00571 } else if (newq[i] < joint[i].min) { 00572 newq[i] = joint[i].min; 00573 joint[0].locked = true; 00574 } 00575 } 00576 } 00577 if (joint[0].locked) { 00578 locked = true; 00579 norm = 0.0; 00580 // compute delta to locked position 00581 for (unsigned int i=0; i<joint->ndof; i++) { 00582 qdot[i] = newq[i] - q[i]; 00583 norm += qdot[i]*qdot[i]; 00584 } 00585 if (norm < KDL::epsilon2) { 00586 // joint didn't move, no need to update the jacobian 00587 callback.lockJoint(q_nr, joint->ndof); 00588 } else { 00589 // solver needs velocity, compute equivalent velocity 00590 for (unsigned int i=0; i<joint->ndof; i++) { 00591 qdot[i] /= timestamp.realTimestep; 00592 } 00593 callback.lockJoint(q_nr, joint->ndof, qdot); 00594 goto end_loop; 00595 } 00596 } else 00597 unlocked++; 00598 } 00599 } 00600 qdot += joint->ndof; 00601 q += joint->ndof; 00602 newq += joint->ndof; 00603 q_nr += joint->ndof; 00604 } 00605 end_loop: 00606 // check if there any other unlocked joint 00607 for ( ; q_nr<m_nq; ) { 00608 Joint_struct* joint = &m_joints[q_nr]; 00609 if (!joint->locked) 00610 unlocked++; 00611 q_nr += joint->ndof; 00612 } 00613 // if all joints have been locked no need to run the solver again 00614 return (unlocked) ? locked : false; 00615 } 00616 00617 void Armature::updateKinematics(const Timestamp& timestamp){ 00618 00619 //Integrate m_qdot 00620 if (!m_finalized) 00621 return; 00622 00623 // the new joint value have been computed already, just copy 00624 memcpy(&m_qKdl(0), &m_newqKdl(0), sizeof(double)*m_qKdl.rows()); 00625 pushCache(timestamp); 00626 updateJacobian(); 00627 // here update the desired output. 00628 // We assume constant desired output for the joint limit constraint, no need to update it. 00629 } 00630 00631 void Armature::updateJacobian() 00632 { 00633 //calculate pose and jacobian 00634 for (unsigned int ee=0; ee<m_nee; ee++) { 00635 m_fksolver->JntToCart(m_qKdl,m_effectors[ee].pose,m_effectors[ee].name,m_root); 00636 m_jacsolver->JntToJac(m_qKdl,*m_jac,m_effectors[ee].name); 00637 // get the jacobian for the base point, to prepare transformation to world reference 00638 changeRefPoint(*m_jac,-m_effectors[ee].pose.p,*m_jac); 00639 //copy to Jq: 00640 e_matrix& Jq = m_JqArray[ee]; 00641 for(unsigned int i=0;i<6;i++) { 00642 for(unsigned int j=0;j<m_nq;j++) 00643 Jq(i,j)=(*m_jac)(i,j); 00644 } 00645 } 00646 // remember that this object has moved 00647 m_updated = true; 00648 } 00649 00650 const Frame& Armature::getPose(const unsigned int ee) 00651 { 00652 if (!m_finalized) 00653 return F_identity; 00654 return (ee >= m_nee) ? F_identity : m_effectors[ee].pose; 00655 } 00656 00657 bool Armature::getRelativeFrame(Frame& result, const std::string& segment_name, const std::string& base_name) 00658 { 00659 if (!m_finalized) 00660 return false; 00661 return (m_fksolver->JntToCart(m_qKdl,result,segment_name,base_name) < 0) ? false : true; 00662 } 00663 00664 void Armature::updateControlOutput(const Timestamp& timestamp) 00665 { 00666 if (!m_finalized) 00667 return; 00668 00669 00670 if (!timestamp.substep && !timestamp.reiterate && timestamp.interpolate) { 00671 popQ(timestamp.cacheTimestamp); 00672 //popConstraints(timestamp.cacheTimestamp); 00673 } 00674 00675 if (!timestamp.substep) { 00676 // save previous joint state for getMaxJointChange() 00677 memcpy(&m_oldqKdl(0), &m_qKdl(0), sizeof(double)*m_qKdl.rows()); 00678 for (unsigned int i=0; i<m_neffector; i++) { 00679 m_effectors[i].oldpose = m_effectors[i].pose; 00680 } 00681 } 00682 00683 // remove all joint lock 00684 for (JointList::iterator jit=m_joints.begin(); jit!=m_joints.end(); ++jit) { 00685 (*jit).locked = false; 00686 } 00687 00688 JointConstraintList::iterator it; 00689 unsigned int iConstraint; 00690 00691 // scan through the constraints and call the callback functions 00692 for (iConstraint=0, it=m_constraints.begin(); it!=m_constraints.end(); it++, iConstraint++) { 00693 JointConstraint_struct* pConstraint = *it; 00694 unsigned int nr, i; 00695 for (i=0, nr = pConstraint->segment->second.q_nr; i<pConstraint->v_nr; i++, nr++) { 00696 *(double*)&pConstraint->value[i].y = m_qKdl(nr); 00697 *(double*)&pConstraint->value[i].ydot = m_qdotKdl(nr); 00698 } 00699 if (pConstraint->function && (pConstraint->substep || (!timestamp.reiterate && !timestamp.substep))) { 00700 (*pConstraint->function)(timestamp, pConstraint->values, pConstraint->v_nr, pConstraint->param); 00701 } 00702 // recompute the weight in any case, that's the most likely modification 00703 for (i=0, nr=pConstraint->y_nr; i<pConstraint->v_nr; i++, nr++) { 00704 m_Wy(nr) = pConstraint->values[i].alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/; 00705 m_ydot(nr)=pConstraint->value[i].yddot+pConstraint->values[i].feedback*(pConstraint->value[i].yd-pConstraint->value[i].y); 00706 } 00707 } 00708 } 00709 00710 bool Armature::setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, double value, double timestep) 00711 { 00712 unsigned int lastid, i; 00713 if (constraintId == CONSTRAINT_ID_ALL) { 00714 constraintId = 0; 00715 lastid = m_nconstraint; 00716 } else if (constraintId < m_nconstraint) { 00717 lastid = constraintId+1; 00718 } else { 00719 return false; 00720 } 00721 for ( ; constraintId<lastid; ++constraintId) { 00722 JointConstraint_struct* pConstraint = m_constraints[constraintId]; 00723 if (valueId == ID_JOINT) { 00724 for (i=0; i<pConstraint->v_nr; i++) { 00725 switch (action) { 00726 case ACT_TOLERANCE: 00727 pConstraint->values[i].tolerance = value; 00728 break; 00729 case ACT_FEEDBACK: 00730 pConstraint->values[i].feedback = value; 00731 break; 00732 case ACT_ALPHA: 00733 pConstraint->values[i].alpha = value; 00734 break; 00735 default: 00736 break; 00737 } 00738 } 00739 } else { 00740 for (i=0; i<pConstraint->v_nr; i++) { 00741 if (valueId == pConstraint->value[i].id) { 00742 switch (action) { 00743 case ACT_VALUE: 00744 pConstraint->value[i].yd = value; 00745 break; 00746 case ACT_VELOCITY: 00747 pConstraint->value[i].yddot = value; 00748 break; 00749 case ACT_TOLERANCE: 00750 pConstraint->values[i].tolerance = value; 00751 break; 00752 case ACT_FEEDBACK: 00753 pConstraint->values[i].feedback = value; 00754 break; 00755 case ACT_ALPHA: 00756 pConstraint->values[i].alpha = value; 00757 break; 00758 case ACT_NONE: 00759 break; 00760 } 00761 } 00762 } 00763 } 00764 if (m_finalized) { 00765 for (i=0; i<pConstraint->v_nr; i++) 00766 m_Wy(pConstraint->y_nr+i) = pConstraint->values[i].alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/; 00767 } 00768 } 00769 return true; 00770 } 00771 00772 } 00773