diff options
Diffstat (limited to 'intern/itasc')
90 files changed, 16057 insertions, 0 deletions
diff --git a/intern/itasc/Armature.cpp b/intern/itasc/Armature.cpp new file mode 100644 index 00000000000..7776b6aa3b6 --- /dev/null +++ b/intern/itasc/Armature.cpp @@ -0,0 +1,775 @@ +/* $Id$ + * Armature.cpp + * + * Created on: Feb 3, 2009 + * Author: benoitbolsee + */ + +#include "Armature.hpp" +#include <algorithm> +#include <string.h> +#include <stdlib.h> + +namespace iTaSC { + +// a joint constraint is characterized by 5 values: tolerance, K, alpha, yd, yddot +static const unsigned int constraintCacheSize = 5; +std::string Armature::m_root = "root"; + +Armature::Armature(): + ControlledObject(), + m_tree(), + m_njoint(0), + m_nconstraint(0), + m_noutput(0), + m_neffector(0), + m_finalized(false), + m_cache(NULL), + m_buf(NULL), + m_qCCh(-1), + m_qCTs(0), + m_yCCh(-1), + m_yCTs(0), + m_qKdl(), + m_oldqKdl(), + m_newqKdl(), + m_qdotKdl(), + m_jac(NULL), + m_armlength(0.0), + m_jacsolver(NULL), + m_fksolver(NULL) +{ +} + +Armature::~Armature() +{ + if (m_jac) + delete m_jac; + if (m_jacsolver) + delete m_jacsolver; + if (m_fksolver) + delete m_fksolver; + for (JointConstraintList::iterator it=m_constraints.begin(); it != m_constraints.end(); it++) { + if (*it != NULL) + delete (*it); + } + if (m_buf) + delete [] m_buf; + m_constraints.clear(); +} + +Armature::JointConstraint_struct::JointConstraint_struct(SegmentMap::const_iterator _segment, unsigned int _y_nr, ConstraintCallback _function, void* _param, bool _freeParam, bool _substep): + segment(_segment), value(), values(), function(_function), y_nr(_y_nr), param(_param), freeParam(_freeParam), substep(_substep) +{ + memset(values, 0, sizeof(values)); + memset(value, 0, sizeof(value)); + values[0].feedback = 20.0; + values[1].feedback = 20.0; + values[2].feedback = 20.0; + values[0].tolerance = 1.0; + values[1].tolerance = 1.0; + values[2].tolerance = 1.0; + values[0].values = &value[0]; + values[1].values = &value[1]; + values[2].values = &value[2]; + values[0].number = 1; + values[1].number = 1; + values[2].number = 1; + switch (segment->second.segment.getJoint().getType()) { + case Joint::RotX: + value[0].id = ID_JOINT_RX; + values[0].id = ID_JOINT_RX; + v_nr = 1; + break; + case Joint::RotY: + value[0].id = ID_JOINT_RY; + values[0].id = ID_JOINT_RY; + v_nr = 1; + break; + case Joint::RotZ: + value[0].id = ID_JOINT_RZ; + values[0].id = ID_JOINT_RZ; + v_nr = 1; + break; + case Joint::TransX: + value[0].id = ID_JOINT_TX; + values[0].id = ID_JOINT_TX; + v_nr = 1; + break; + case Joint::TransY: + value[0].id = ID_JOINT_TY; + values[0].id = ID_JOINT_TY; + v_nr = 1; + break; + case Joint::TransZ: + value[0].id = ID_JOINT_TZ; + values[0].id = ID_JOINT_TZ; + v_nr = 1; + break; + case Joint::Sphere: + values[0].id = value[0].id = ID_JOINT_RX; + values[1].id = value[1].id = ID_JOINT_RY; + values[2].id = value[2].id = ID_JOINT_RZ; + v_nr = 3; + break; + case Joint::Swing: + values[0].id = value[0].id = ID_JOINT_RX; + values[1].id = value[1].id = ID_JOINT_RZ; + v_nr = 2; + break; + case Joint::None: + break; + } +} + +Armature::JointConstraint_struct::~JointConstraint_struct() +{ + if (freeParam && param) + free(param); +} + +void Armature::initCache(Cache *_cache) +{ + m_cache = _cache; + m_qCCh = -1; + m_yCCh = -1; + m_buf = NULL; + if (m_cache) { + // add a special channel for the joint + m_qCCh = m_cache->addChannel(this, "q", m_qKdl.rows()*sizeof(double)); +#if 0 + // for the constraints, instead of creating many different channels, we will + // create a single channel for all the constraints + if (m_nconstraint) { + m_yCCh = m_cache->addChannel(this, "y", m_nconstraint*constraintCacheSize*sizeof(double)); + m_buf = new double[m_nconstraint*constraintCacheSize]; + } + // store the initial cache position at timestamp 0 + pushConstraints(0); +#endif + pushQ(0); + } +} + +void Armature::pushQ(CacheTS timestamp) +{ + if (m_qCCh >= 0) { + // try to keep the cache if the joints are the same + m_cache->addCacheVectorIfDifferent(this, m_qCCh, timestamp, &m_qKdl(0), m_qKdl.rows(), KDL::epsilon); + m_qCTs = timestamp; + } +} + +/* return true if a m_cache position was loaded */ +bool Armature::popQ(CacheTS timestamp) +{ + if (m_qCCh >= 0) { + double* item; + item = (double*)m_cache->getPreviousCacheItem(this, m_qCCh, ×tamp); + if (item && m_qCTs != timestamp) { + double& q = m_qKdl(0); + memcpy(&q, item, m_qKdl.rows()*sizeof(q)); + m_qCTs = timestamp; + // changing the joint => recompute the jacobian + updateJacobian(); + } + return (item) ? true : false; + } + return true; +} +#if 0 +void Armature::pushConstraints(CacheTS timestamp) +{ + if (m_yCCh >= 0) { + double *buf = NULL; + if (m_nconstraint) { + double *item = m_buf; + for (unsigned int i=0; i<m_nconstraint; i++) { + JointConstraint_struct* pConstraint = m_constraints[i]; + *item++ = pConstraint->values.feedback; + *item++ = pConstraint->values.tolerance; + *item++ = pConstraint->value.yd; + *item++ = pConstraint->value.yddot; + *item++ = pConstraint->values.alpha; + } + } + m_cache->addCacheVectorIfDifferent(this, m_yCCh, timestamp, m_buf, m_nconstraint*constraintCacheSize, KDL::epsilon); + m_yCTs = timestamp; + } +} + +/* return true if a cache position was loaded */ +bool Armature::popConstraints(CacheTS timestamp) +{ + if (m_yCCh >= 0) { + double *item = (double*)m_cache->getPreviousCacheItem(this, m_yCCh, ×tamp); + if (item && m_yCTs != timestamp) { + for (unsigned int i=0; i<m_nconstraint; i++) { + JointConstraint_struct* pConstraint = m_constraints[i]; + if (pConstraint->function != Joint1DOFLimitCallback) { + pConstraint->values.feedback = *item++; + pConstraint->values.tolerance = *item++; + pConstraint->value.yd = *item++; + pConstraint->value.yddot = *item++; + pConstraint->values.alpha = *item++; + } else { + item += constraintCacheSize; + } + } + m_yCTs = timestamp; + } + return (item) ? true : false; + } + return true; +} +#endif + +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) +{ + if (m_finalized) + return false; + + Segment segment(joint, f_tip, M); + if (!m_tree.addSegment(segment, segment_name, hook_name)) + return false; + int ndof = joint.getNDof(); + for (int dof=0; dof<ndof; dof++) { + Joint_struct js(joint.getType(), ndof, (&q_rest)[dof]); + m_joints.push_back(js); + } + m_njoint+=ndof; + return true; +} + +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) +{ + SegmentMap::const_iterator sit = m_tree.getSegment(name); + if (sit == m_tree.getSegments().end()) + return false; + p_joint = &sit->second.segment.getJoint(); + if (q_size < p_joint->getNDof()) + return false; + p_tip = &sit->second.segment.getFrameToTip(); + for (unsigned int dof=0; dof<p_joint->getNDof(); dof++) { + (&q_rest)[dof] = m_joints[sit->second.q_nr+dof].rest; + (&q)[dof] = m_qKdl(sit->second.q_nr+dof); + } + return true; +} + +double Armature::getMaxJointChange() +{ + if (!m_finalized) + return 0.0; + double maxJoint = 0.0; + for (unsigned int i=0; i<m_njoint; i++) { + // this is a very rough calculation, it doesn't work well for spherical joint + double joint = fabs(m_oldqKdl(i)-m_qKdl(i)); + if (maxJoint < joint) + maxJoint = joint; + } + return maxJoint; +} + +double Armature::getMaxEndEffectorChange() +{ + if (!m_finalized) + return 0.0; + double maxDelta = 0.0; + double delta; + Twist twist; + for (unsigned int i = 0; i<m_neffector; i++) { + twist = diff(m_effectors[i].pose, m_effectors[i].oldpose); + delta = twist.rot.Norm(); + if (delta > maxDelta) + maxDelta = delta; + delta = twist.vel.Norm(); + if (delta > maxDelta) + maxDelta = delta; + } + return maxDelta; +} + +int Armature::addConstraint(const std::string& segment_name, ConstraintCallback _function, void* _param, bool _freeParam, bool _substep) +{ + SegmentMap::const_iterator segment_it = m_tree.getSegment(segment_name); + // not suitable for NDof joints + if (segment_it == m_tree.getSegments().end()) { + if (_freeParam && _param) + free(_param); + return -1; + } + JointConstraintList::iterator constraint_it; + JointConstraint_struct* pConstraint; + int iConstraint; + for (iConstraint=0, constraint_it=m_constraints.begin(); constraint_it != m_constraints.end(); constraint_it++, iConstraint++) { + pConstraint = *constraint_it; + if (pConstraint->segment == segment_it) { + // redefining a constraint + if (pConstraint->freeParam && pConstraint->param) { + free(pConstraint->param); + } + pConstraint->function = _function; + pConstraint->param = _param; + pConstraint->freeParam = _freeParam; + pConstraint->substep = _substep; + return iConstraint; + } + } + if (m_finalized) { + if (_freeParam && _param) + free(_param); + return -1; + } + // new constraint, append + pConstraint = new JointConstraint_struct(segment_it, m_noutput, _function, _param, _freeParam, _substep); + m_constraints.push_back(pConstraint); + m_noutput += pConstraint->v_nr; + return m_nconstraint++; +} + +int Armature::addLimitConstraint(const std::string& segment_name, unsigned int dof, double _min, double _max) +{ + SegmentMap::const_iterator segment_it = m_tree.getSegment(segment_name); + if (segment_it == m_tree.getSegments().end()) + return -1; + const Joint& joint = segment_it->second.segment.getJoint(); + if (joint.getNDof() != 1 && joint.getType() != Joint::Swing) { + // not suitable for Sphere joints + return -1; + } + if ((joint.getNDof() == 1 && dof > 0) || (joint.getNDof() == 2 && dof > 1)) + return -1; + if (joint.getType() < Joint::TransX || joint.getType() == Joint::Swing) { + // for rotation joint, the limit is given in degree, convert to radian + _min *= KDL::deg2rad; + _max *= KDL::deg2rad; + } + Joint_struct& p_joint = m_joints[segment_it->second.q_nr+dof]; + p_joint.min = _min; + p_joint.max = _max; + p_joint.useLimit = true; + return 0; +} + +int Armature::addEndEffector(const std::string& name) +{ + const SegmentMap& segments = m_tree.getSegments(); + if (segments.find(name) == segments.end()) + return -1; + + EffectorList::const_iterator it; + int ee; + for (it=m_effectors.begin(), ee=0; it!=m_effectors.end(); it++, ee++) { + if (it->name == name) + return ee; + } + if (m_finalized) + return -1; + Effector_struct effector(name); + m_effectors.push_back(effector); + return m_neffector++; +} + +void Armature::finalize() +{ + unsigned int i, j, c; + if (m_finalized) + return; + initialize(m_njoint, m_noutput, m_neffector); + for (i=c=0; i<m_nconstraint; i++) { + JointConstraint_struct* pConstraint = m_constraints[i]; + for (j=0; j<pConstraint->v_nr; j++, c++) { + m_Cq(c,pConstraint->segment->second.q_nr+j) = 1.0; + m_Wy(c) = pConstraint->values[j].alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/; + } + } + m_jacsolver= new KDL::TreeJntToJacSolver(m_tree); + m_fksolver = new KDL::TreeFkSolverPos_recursive(m_tree); + m_jac = new Jacobian(m_njoint); + m_qKdl.resize(m_njoint); + m_oldqKdl.resize(m_njoint); + m_newqKdl.resize(m_njoint); + m_qdotKdl.resize(m_njoint); + for (i=0; i<m_njoint; i++) { + m_newqKdl(i) = m_oldqKdl(i) = m_qKdl(i) = m_joints[i].rest; + } + updateJacobian(); + // estimate the maximum size of the robot arms + double length; + m_armlength = 0.0; + for (i=0; i<m_neffector; i++) { + length = 0.0; + KDL::SegmentMap::const_iterator sit = m_tree.getSegment(m_effectors[i].name); + while (sit->first != "root") { + Frame tip = sit->second.segment.pose(m_qKdl(sit->second.q_nr)); + length += tip.p.Norm(); + sit = sit->second.parent; + } + if (length > m_armlength) + m_armlength = length; + } + if (m_armlength < KDL::epsilon) + m_armlength = KDL::epsilon; + m_finalized = true; +} + +void Armature::pushCache(const Timestamp& timestamp) +{ + if (!timestamp.substep && timestamp.cache) { + pushQ(timestamp.cacheTimestamp); + //pushConstraints(timestamp.cacheTimestamp); + } +} + +bool Armature::setJointArray(const KDL::JntArray& joints) +{ + if (!m_finalized) + return false; + if (joints.rows() != m_qKdl.rows()) + return false; + m_qKdl = joints; + updateJacobian(); + return true; +} + +const KDL::JntArray& Armature::getJointArray() +{ + return m_qKdl; +} + +bool Armature::updateJoint(const Timestamp& timestamp, JointLockCallback& callback) +{ + if (!m_finalized) + return false; + + // integration and joint limit + // for spherical joint we must use a more sophisticated method + unsigned int q_nr; + double* qdot=&m_qdotKdl(0); + double* q=&m_qKdl(0); + double* newq=&m_newqKdl(0); + double norm, qx, qz, CX, CZ, sx, sz; + bool locked = false; + int unlocked = 0; + + for (q_nr=0; q_nr<m_nq; ++q_nr) + m_qdotKdl(q_nr)=m_qdot(q_nr); + + for (q_nr=0; q_nr<m_nq; ) { + Joint_struct* joint = &m_joints[q_nr]; + if (!joint->locked) { + switch (joint->type) { + case KDL::Joint::Swing: + { + KDL::Rotation base = KDL::Rot(KDL::Vector(q[0],0.0,q[1])); + (base*KDL::Rot(KDL::Vector(qdot[0],0.0,qdot[1])*timestamp.realTimestep)).GetXZRot().GetValue(newq); + if (joint[0].useLimit) { + if (joint[1].useLimit) { + // elliptical limit + sx = sz = 1.0; + qx = newq[0]; + qz = newq[1]; + // determine in which quadrant we are + if (qx > 0.0 && qz > 0.0) { + CX = joint[0].max; + CZ = joint[1].max; + } else if (qx <= 0.0 && qz > 0.0) { + CX = -joint[0].min; + CZ = joint[1].max; + qx = -qx; + sx = -1.0; + } else if (qx <= 0.0 && qz <= 0.0) { + CX = -joint[0].min; + CZ = -joint[1].min; + qx = -qx; + qz = -qz; + sx = sz = -1.0; + } else { + CX = joint[0].max; + CZ = -joint[0].min; + qz = -qz; + sz = -1.0; + } + if (CX < KDL::epsilon || CZ < KDL::epsilon) { + // quadrant is degenerated + if (qx > CX) { + newq[0] = CX*sx; + joint[0].locked = true; + } + if (qz > CZ) { + newq[1] = CZ*sz; + joint[0].locked = true; + } + } else { + // general case + qx /= CX; + qz /= CZ; + norm = KDL::sqrt(KDL::sqr(qx)+KDL::sqr(qz)); + if (norm > 1.0) { + norm = 1.0/norm; + newq[0] = qx*norm*CX*sx; + newq[1] = qz*norm*CZ*sz; + joint[0].locked = true; + } + } + } else { + // limit on X only + qx = newq[0]; + if (qx > joint[0].max) { + newq[0] = joint[0].max; + joint[0].locked = true; + } else if (qx < joint[0].min) { + newq[0] = joint[0].min; + joint[0].locked = true; + } + } + } else if (joint[1].useLimit) { + // limit on Z only + qz = newq[1]; + if (qz > joint[1].max) { + newq[1] = joint[1].max; + joint[0].locked = true; + } else if (qz < joint[1].min) { + newq[1] = joint[1].min; + joint[0].locked = true; + } + } + if (joint[0].locked) { + // check the difference from previous position + locked = true; + norm = KDL::sqr(newq[0]-q[0])+KDL::sqr(newq[1]-q[1]); + if (norm < KDL::epsilon2) { + // joint didn't move, no need to update the jacobian + callback.lockJoint(q_nr, 2); + } else { + // joint moved, compute the corresponding velocity + double deltaq[2]; + (base.Inverse()*KDL::Rot(KDL::Vector(newq[0],0.0,newq[1]))).GetXZRot().GetValue(deltaq); + deltaq[0] /= timestamp.realTimestep; + deltaq[1] /= timestamp.realTimestep; + callback.lockJoint(q_nr, 2, deltaq); + // no need to update the other joints, it will be done after next rerun + goto end_loop; + } + } else + unlocked++; + break; + } + case KDL::Joint::Sphere: + { + (KDL::Rot(KDL::Vector(q))*KDL::Rot(KDL::Vector(qdot)*timestamp.realTimestep)).GetRot().GetValue(newq); + // no limit on this joint + unlocked++; + break; + } + default: + for (unsigned int i=0; i<joint->ndof; i++) { + newq[i] = q[i]+qdot[i]*timestamp.realTimestep; + if (joint[i].useLimit) { + if (newq[i] > joint[i].max) { + newq[i] = joint[i].max; + joint[0].locked = true; + } else if (newq[i] < joint[i].min) { + newq[i] = joint[i].min; + joint[0].locked = true; + } + } + } + if (joint[0].locked) { + locked = true; + norm = 0.0; + // compute delta to locked position + for (unsigned int i=0; i<joint->ndof; i++) { + qdot[i] = newq[i] - q[i]; + norm += qdot[i]*qdot[i]; + } + if (norm < KDL::epsilon2) { + // joint didn't move, no need to update the jacobian + callback.lockJoint(q_nr, joint->ndof); + } else { + // solver needs velocity, compute equivalent velocity + for (unsigned int i=0; i<joint->ndof; i++) { + qdot[i] /= timestamp.realTimestep; + } + callback.lockJoint(q_nr, joint->ndof, qdot); + goto end_loop; + } + } else + unlocked++; + } + } + qdot += joint->ndof; + q += joint->ndof; + newq += joint->ndof; + q_nr += joint->ndof; + } +end_loop: + // check if there any other unlocked joint + for ( ; q_nr<m_nq; ) { + Joint_struct* joint = &m_joints[q_nr]; + if (!joint->locked) + unlocked++; + q_nr += joint->ndof; + } + // if all joints have been locked no need to run the solver again + return (unlocked) ? locked : false; +} + +void Armature::updateKinematics(const Timestamp& timestamp){ + + //Integrate m_qdot + if (!m_finalized) + return; + + // the new joint value have been computed already, just copy + memcpy(&m_qKdl(0), &m_newqKdl(0), sizeof(double)*m_qKdl.rows()); + pushCache(timestamp); + updateJacobian(); + // here update the desired output. + // We assume constant desired output for the joint limit constraint, no need to update it. +} + +void Armature::updateJacobian() +{ + //calculate pose and jacobian + for (unsigned int ee=0; ee<m_nee; ee++) { + m_fksolver->JntToCart(m_qKdl,m_effectors[ee].pose,m_effectors[ee].name,m_root); + m_jacsolver->JntToJac(m_qKdl,*m_jac,m_effectors[ee].name); + // get the jacobian for the base point, to prepare transformation to world reference + changeRefPoint(*m_jac,-m_effectors[ee].pose.p,*m_jac); + //copy to Jq: + e_matrix& Jq = m_JqArray[ee]; + for(unsigned int i=0;i<6;i++) { + for(unsigned int j=0;j<m_nq;j++) + Jq(i,j)=(*m_jac)(i,j); + } + } + // remember that this object has moved + m_updated = true; +} + +const Frame& Armature::getPose(const unsigned int ee) +{ + if (!m_finalized) + return F_identity; + return (ee >= m_nee) ? F_identity : m_effectors[ee].pose; +} + +bool Armature::getRelativeFrame(Frame& result, const std::string& segment_name, const std::string& base_name) +{ + if (!m_finalized) + return false; + return (m_fksolver->JntToCart(m_qKdl,result,segment_name,base_name) < 0) ? false : true; +} + +void Armature::updateControlOutput(const Timestamp& timestamp) +{ + if (!m_finalized) + return; + + + if (!timestamp.substep && !timestamp.reiterate && timestamp.interpolate) { + popQ(timestamp.cacheTimestamp); + //popConstraints(timestamp.cacheTimestamp); + } + + if (!timestamp.substep) { + // save previous joint state for getMaxJointChange() + memcpy(&m_oldqKdl(0), &m_qKdl(0), sizeof(double)*m_qKdl.rows()); + for (unsigned int i=0; i<m_neffector; i++) { + m_effectors[i].oldpose = m_effectors[i].pose; + } + } + + // remove all joint lock + for (JointList::iterator jit=m_joints.begin(); jit!=m_joints.end(); ++jit) { + (*jit).locked = false; + } + + JointConstraintList::iterator it; + unsigned int iConstraint; + + // scan through the constraints and call the callback functions + for (iConstraint=0, it=m_constraints.begin(); it!=m_constraints.end(); it++, iConstraint++) { + JointConstraint_struct* pConstraint = *it; + unsigned int nr, i; + for (i=0, nr = pConstraint->segment->second.q_nr; i<pConstraint->v_nr; i++, nr++) { + *(double*)&pConstraint->value[i].y = m_qKdl(nr); + *(double*)&pConstraint->value[i].ydot = m_qdotKdl(nr); + } + if (pConstraint->function && (pConstraint->substep || (!timestamp.reiterate && !timestamp.substep))) { + (*pConstraint->function)(timestamp, pConstraint->values, pConstraint->v_nr, pConstraint->param); + } + // recompute the weight in any case, that's the most likely modification + for (i=0, nr=pConstraint->y_nr; i<pConstraint->v_nr; i++, nr++) { + m_Wy(nr) = pConstraint->values[i].alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/; + m_ydot(nr)=pConstraint->value[i].yddot+pConstraint->values[i].feedback*(pConstraint->value[i].yd-pConstraint->value[i].y); + } + } +} + +bool Armature::setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, double value, double timestep) +{ + unsigned int lastid, i; + if (constraintId == CONSTRAINT_ID_ALL) { + constraintId = 0; + lastid = m_nconstraint; + } else if (constraintId < m_nconstraint) { + lastid = constraintId+1; + } else { + return false; + } + for ( ; constraintId<lastid; ++constraintId) { + JointConstraint_struct* pConstraint = m_constraints[constraintId]; + if (valueId == ID_JOINT) { + for (i=0; i<pConstraint->v_nr; i++) { + switch (action) { + case ACT_TOLERANCE: + pConstraint->values[i].tolerance = value; + break; + case ACT_FEEDBACK: + pConstraint->values[i].feedback = value; + break; + case ACT_ALPHA: + pConstraint->values[i].alpha = value; + break; + default: + break; + } + } + } else { + for (i=0; i<pConstraint->v_nr; i++) { + if (valueId == pConstraint->value[i].id) { + switch (action) { + case ACT_VALUE: + pConstraint->value[i].yd = value; + break; + case ACT_VELOCITY: + pConstraint->value[i].yddot = value; + break; + case ACT_TOLERANCE: + pConstraint->values[i].tolerance = value; + break; + case ACT_FEEDBACK: + pConstraint->values[i].feedback = value; + break; + case ACT_ALPHA: + pConstraint->values[i].alpha = value; + break; + case ACT_NONE: + break; + } + } + } + } + if (m_finalized) { + for (i=0; i<pConstraint->v_nr; i++) + m_Wy(pConstraint->y_nr+i) = pConstraint->values[i].alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/; + } + } + return true; +} + +} + diff --git a/intern/itasc/Armature.hpp b/intern/itasc/Armature.hpp new file mode 100644 index 00000000000..312ca1b28c3 --- /dev/null +++ b/intern/itasc/Armature.hpp @@ -0,0 +1,137 @@ +/* $Id: Armature.hpp 20853 2009-06-13 12:29:46Z ben2610 $ + * Armature.hpp + * + * Created on: Feb 3, 2009 + * Author: benoitbolsee + */ + +#ifndef ARMATURE_HPP_ +#define ARMATURE_HPP_ + +#include "ControlledObject.hpp" +#include "ConstraintSet.hpp" +#include "kdl/treejnttojacsolver.hpp" +#include "kdl/treefksolverpos_recursive.hpp" +#include <vector> + +namespace iTaSC { + +class Armature: public iTaSC::ControlledObject { +public: + Armature(); + virtual ~Armature(); + + bool addSegment(const std::string& segment_name, const std::string& hook_name, const Joint& joint, const double& q_rest, const Frame& f_tip=F_identity, const Inertia& M = Inertia::Zero()); + // general purpose constraint on joint + int addConstraint(const std::string& segment_name, ConstraintCallback _function, void* _param=NULL, bool _freeParam=false, bool _substep=false); + // specific limit constraint on joint + int addLimitConstraint(const std::string& segment_name, unsigned int dof, double _min, double _max); + double getMaxJointChange(); + double getMaxEndEffectorChange(); + bool getSegment(const std::string& segment_name, const unsigned int q_size, const Joint* &p_joint, double &q_rest, double &q, const Frame* &p_tip); + bool getRelativeFrame(Frame& result, const std::string& segment_name, const std::string& base_name=m_root); + + virtual void finalize(); + + virtual int addEndEffector(const std::string& name); + virtual const Frame& getPose(const unsigned int end_effector); + virtual bool updateJoint(const Timestamp& timestamp, JointLockCallback& callback); + virtual void updateKinematics(const Timestamp& timestamp); + virtual void pushCache(const Timestamp& timestamp); + virtual void updateControlOutput(const Timestamp& timestamp); + virtual bool setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, double value, double timestep=0.0); + virtual void initCache(Cache *_cache); + virtual bool setJointArray(const KDL::JntArray& joints); + virtual const KDL::JntArray& getJointArray(); + + virtual double getArmLength() + { + return m_armlength; + } + + struct Effector_struct { + std::string name; + Frame oldpose; + Frame pose; + Effector_struct(const std::string& _name) {name = _name; oldpose = pose = F_identity;} + }; + typedef std::vector<Effector_struct> EffectorList; + + enum ID { + ID_JOINT=1, + ID_JOINT_RX=2, + ID_JOINT_RY=3, + ID_JOINT_RZ=4, + ID_JOINT_TX=2, + ID_JOINT_TY=3, + ID_JOINT_TZ=4, + }; + struct JointConstraint_struct { + SegmentMap::const_iterator segment; + ConstraintSingleValue value[3]; + ConstraintValues values[3]; + ConstraintCallback function; + unsigned int v_nr; + unsigned int y_nr; // first coordinate of constraint in Y vector + void* param; + bool freeParam; + bool substep; + JointConstraint_struct(SegmentMap::const_iterator _segment, unsigned int _y_nr, ConstraintCallback _function, void* _param, bool _freeParam, bool _substep); + ~JointConstraint_struct(); + }; + typedef std::vector<JointConstraint_struct*> JointConstraintList; + + struct Joint_struct { + KDL::Joint::JointType type; + unsigned short ndof; + bool useLimit; + bool locked; + double rest; + double min; + double max; + + Joint_struct(KDL::Joint::JointType _type, unsigned int _ndof, double _rest) : + type(_type), ndof(_ndof), rest(_rest) { useLimit=locked=false; min=0.0; max=0.0; } + }; + typedef std::vector<Joint_struct> JointList; + +protected: + virtual void updateJacobian(); + +private: + static std::string m_root; + Tree m_tree; + unsigned int m_njoint; + unsigned int m_nconstraint; + unsigned int m_noutput; + unsigned int m_neffector; + bool m_finalized; + Cache* m_cache; + double *m_buf; + int m_qCCh; + CacheTS m_qCTs; + int m_yCCh; + CacheTS m_yCTs; + JntArray m_qKdl; + JntArray m_oldqKdl; + JntArray m_newqKdl; + JntArray m_qdotKdl; + Jacobian* m_jac; + double m_armlength; + + KDL::TreeJntToJacSolver* m_jacsolver; + KDL::TreeFkSolverPos_recursive* m_fksolver; + EffectorList m_effectors; + JointConstraintList m_constraints; + JointList m_joints; + + void pushQ(CacheTS timestamp); + bool popQ(CacheTS timestamp); + //void pushConstraints(CacheTS timestamp); + //bool popConstraints(CacheTS timestamp); + +}; + +} + +#endif /* ARMATURE_HPP_ */ diff --git a/intern/itasc/CMakeLists.txt b/intern/itasc/CMakeLists.txt new file mode 100644 index 00000000000..405d74d17ac --- /dev/null +++ b/intern/itasc/CMakeLists.txt @@ -0,0 +1,32 @@ +# $Id: CMakeLists.txt 19905 2009-04-23 13:29:54Z ben2610 $ +# ***** BEGIN GPL LICENSE BLOCK ***** +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software Foundation, +# Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +# +# The Original Code is Copyright (C) 2006, Blender Foundation +# All rights reserved. +# +# The Original Code is: all of this file. +# +# Contributor(s): Jacques Beaurain. +# +# ***** END GPL LICENSE BLOCK ***** + +SET(INC ../../extern/Eigen2) + +FILE(GLOB SRC *.cpp kdl/*.cpp kdl/utilities/*.cpp) + +BLENDERLIB(bf_ITASC "${SRC}" "${INC}") +#, libtype=['blender'], priority = [10] ) diff --git a/intern/itasc/Cache.cpp b/intern/itasc/Cache.cpp new file mode 100644 index 00000000000..ccd9cef4655 --- /dev/null +++ b/intern/itasc/Cache.cpp @@ -0,0 +1,620 @@ +/* $Id$ + * Cache.cpp + * + * Created on: Feb 24, 2009 + * Author: benoit bolsee + */ +#include <string.h> +#include <assert.h> +#include <math.h> +#include <stdlib.h> +#include "Cache.hpp" + +namespace iTaSC { + +CacheEntry::~CacheEntry() +{ + for (unsigned int id=0; id < m_count; id++) + m_channelArray[id].clear(); + if (m_channelArray) + free(m_channelArray); +} + +CacheItem *CacheChannel::_findBlock(CacheBuffer *buffer, unsigned short timeOffset, unsigned int *retBlock) +{ + // the timestamp is necessarily in this buffer + unsigned int lowBlock, highBlock, midBlock; + if (timeOffset <= buffer->lookup[0].m_timeOffset) { + // special case: the item is in the first block, search from start + *retBlock = 0; + return &buffer->m_firstItem; + } + // general case, the item is in the middle of the buffer + // before doing a dycotomic search, we will assume that timestamp + // are regularly spaced so that we can try to locate the block directly + highBlock = buffer->m_lastItemPositionW>>m_positionToBlockShiftW; + lowBlock = midBlock = (timeOffset*highBlock)/(buffer->m_lastTimestamp-buffer->m_firstTimestamp); + // give some space for security + if (lowBlock > 0) + lowBlock--; + if (timeOffset <= buffer->lookup[lowBlock].m_timeOffset) { + // bad guess, but we know this block is a good high block, just use it + highBlock = lowBlock; + lowBlock = 0; + } else { + // ok, good guess, now check the high block, give some space + if (midBlock < highBlock) + midBlock++; + if (timeOffset <= buffer->lookup[midBlock].m_timeOffset) { + // good guess, keep that block as the high block + highBlock = midBlock; + } + } + // the item is in a different block, do a dycotomic search + // the timestamp is alway > lowBlock and <= highBlock + while (1) { + midBlock = (lowBlock+highBlock)/2; + if (midBlock == lowBlock) { + // low block and high block are contigous, we can start search from the low block + break; + } else if (timeOffset <= buffer->lookup[midBlock].m_timeOffset) { + highBlock = midBlock; + } else { + lowBlock = midBlock; + } + } + assert (lowBlock != highBlock); + *retBlock = highBlock; + return CACHE_BLOCK_ITEM_ADDR(this,buffer,lowBlock); +} + +void CacheChannel::clear() +{ + CacheBuffer *buffer, *next; + for (buffer=m_firstBuffer; buffer != 0; buffer = next) { + next = buffer->m_next; + free(buffer); + } + m_firstBuffer = NULL; + m_lastBuffer = NULL; + if (initItem) { + free(initItem); + initItem = NULL; + } +} + +CacheBuffer* CacheChannel::allocBuffer() +{ + CacheBuffer* buffer; + if (!m_busy) + return NULL; + buffer = (CacheBuffer*)malloc(CACHE_BUFFER_HEADER_SIZE+(m_bufferSizeW<<2)); + if (buffer) { + memset(buffer, 0, CACHE_BUFFER_HEADER_SIZE); + } + return buffer; +} + +CacheItem* CacheChannel::findItemOrLater(unsigned int timestamp, CacheBuffer **rBuffer) +{ + CacheBuffer* buffer; + CacheItem *item, *limit; + if (!m_busy) + return NULL; + if (timestamp == 0 && initItem) { + *rBuffer = NULL; + return initItem; + } + for (buffer=m_firstBuffer; buffer; buffer = buffer->m_next) { + if (buffer->m_firstFreePositionW == 0) + // buffer is empty, this must be the last and we didn't find the timestamp + return NULL; + if (timestamp < buffer->m_firstTimestamp) { + *rBuffer = buffer; + return &buffer->m_firstItem; + } + if (timestamp <= buffer->m_lastTimestamp) { + // the timestamp is necessarily in this buffer + unsigned short timeOffset = (unsigned short)(timestamp-buffer->m_firstTimestamp); + unsigned int highBlock; + item = _findBlock(buffer, timeOffset, &highBlock); + // now we do a linear search until we find a timestamp that is equal or higher + // we should normally always find an item but let's put a limit just in case + limit = CACHE_BLOCK_ITEM_ADDR(this,buffer,highBlock); + while (item<=limit && item->m_timeOffset < timeOffset ) + item = CACHE_NEXT_ITEM(item); + assert(item<=limit); + *rBuffer = buffer; + return item; + } + // search in next buffer + } + return NULL; +} + +CacheItem* CacheChannel::findItemEarlier(unsigned int timestamp, CacheBuffer **rBuffer) +{ + CacheBuffer *buffer, *prevBuffer; + CacheItem *item, *limit, *prevItem; + if (!m_busy) + return NULL; + if (timestamp == 0) + return NULL; + for (prevBuffer=NULL, buffer=m_firstBuffer; buffer; prevBuffer = buffer, buffer = buffer->m_next) { + if (buffer->m_firstFreePositionW == 0) + // buffer is empty, this must be the last and we didn't find the timestamp + return NULL; + if (timestamp <= buffer->m_firstTimestamp) { + if (prevBuffer == NULL) { + // no item before, except the initial item + *rBuffer = NULL; + return initItem; + } + // the item is necessarily the last one of previous buffer + *rBuffer = prevBuffer; + return CACHE_ITEM_ADDR(prevBuffer,prevBuffer->m_lastItemPositionW); + } + if (timestamp <= buffer->m_lastTimestamp) { + // the timestamp is necessarily in this buffer + unsigned short timeOffset = (unsigned short)(timestamp-buffer->m_firstTimestamp); + unsigned int highBlock; + item = _findBlock(buffer, timeOffset, &highBlock); + // now we do a linear search until we find a timestamp that is equal or higher + // we should normally always find an item but let's put a limit just in case + limit = CACHE_BLOCK_ITEM_ADDR(this,buffer,highBlock); + prevItem = NULL; + while (item<=limit && item->m_timeOffset < timeOffset) { + prevItem = item; + item = CACHE_NEXT_ITEM(item); + } + assert(item<=limit && prevItem!=NULL); + *rBuffer = buffer; + return prevItem; + } + // search in next buffer + } + // pass all buffer, the last item is the last item of the last buffer + if (prevBuffer == NULL) { + // no item before, except the initial item + *rBuffer = NULL; + return initItem; + } + // the item is necessarily the last one of previous buffer + *rBuffer = prevBuffer; + return CACHE_ITEM_ADDR(prevBuffer,prevBuffer->m_lastItemPositionW); +} + + +Cache::Cache() +{ +} + +Cache::~Cache() +{ + CacheMap::iterator it; + for (it=m_cache.begin(); it!=m_cache.end(); it=m_cache.begin()) { + deleteDevice(it->first); + } +} + +int Cache::addChannel(const void *device, const char *name, unsigned int maxItemSize) +{ + CacheMap::iterator it = m_cache.find(device); + CacheEntry *entry; + CacheChannel *channel; + unsigned int id; + + if (maxItemSize > 0x3FFF0) + return -1; + + if (it == m_cache.end()) { + // device does not exist yet, create a new entry + entry = new CacheEntry(); + if (entry == NULL) + return -1; + if (!m_cache.insert(CacheMap::value_type(device,entry)).second) + return -1; + } else { + entry = it->second; + } + // locate a channel with the same name and reuse + for (channel=entry->m_channelArray, id=0; id<entry->m_count; id++, channel++) { + if (channel->m_busy && !strcmp(name, channel->m_name)) { + // make this channel free again + deleteChannel(device, id); + // there can only be one channel with the same name + break; + } + } + for (channel=entry->m_channelArray, id=0; id<entry->m_count; id++, channel++) { + // locate a free channel + if (!channel->m_busy) + break; + } + if (id == entry->m_count) { + // no channel free, create new channels + int newcount = entry->m_count + CACHE_CHANNEL_EXTEND_SIZE; + channel = (CacheChannel*)realloc(entry->m_channelArray, newcount*sizeof(CacheChannel)); + if (channel == NULL) + return -1; + entry->m_channelArray = channel; + memset(&entry->m_channelArray[entry->m_count], 0, CACHE_CHANNEL_EXTEND_SIZE*sizeof(CacheChannel)); + entry->m_count = newcount; + channel = &entry->m_channelArray[id]; + } + // compute the optimal buffer size + // The buffer size must be selected so that + // - it does not contain more than 1630 items (=1s of cache assuming 25 items per second) + // - it contains at least one item + // - it's not bigger than 256kb and preferably around 32kb + // - it a multiple of 4 + unsigned int bufSize = 1630*(maxItemSize+4); + if (bufSize >= CACHE_DEFAULT_BUFFER_SIZE) + bufSize = CACHE_DEFAULT_BUFFER_SIZE; + if (bufSize < maxItemSize+16) + bufSize = maxItemSize+16; + bufSize = (bufSize + 3) & ~0x3; + // compute block size and offset bit mask + // the block size is computed so that + // - it is a power of 2 + // - there is at least one item per block + // - there is no more than CACHE_LOOKUP_TABLE_SIZE blocks per buffer + unsigned int blockSize = bufSize/CACHE_LOOKUP_TABLE_SIZE; + if (blockSize < maxItemSize+12) + blockSize = maxItemSize+12; + // find the power of 2 that is immediately larger than blockSize + unsigned int m; + unsigned int pwr2Size = blockSize; + while ((m = (pwr2Size & (pwr2Size-1))) != 0) + pwr2Size = m; + blockSize = (pwr2Size < blockSize) ? pwr2Size<<1 : pwr2Size; + // convert byte size to word size because all positions and size are expressed in 32 bit words + blockSize >>= 2; + channel->m_blockSizeW = blockSize; + channel->m_bufferSizeW = bufSize>>2; + channel->m_firstBuffer = NULL; + channel->m_lastBuffer = NULL; + channel->m_busy = 1; + channel->initItem = NULL; + channel->m_maxItemSizeB = maxItemSize; + strncpy(channel->m_name, name, sizeof(channel->m_name)); + channel->m_name[sizeof(channel->m_name)-1] = 0; + channel->m_positionToOffsetMaskW = (blockSize-1); + for (m=0; blockSize!=1; m++, blockSize>>=1); + channel->m_positionToBlockShiftW = m; + return (int)id; +} + +int Cache::deleteChannel(const void *device, int id) +{ + CacheMap::iterator it = m_cache.find(device); + CacheEntry *entry; + + if (it == m_cache.end()) { + // device does not exist + return -1; + } + entry = it->second; + if (id < 0 || id >= (int)entry->m_count || !entry->m_channelArray[id].m_busy) + return -1; + entry->m_channelArray[id].clear(); + entry->m_channelArray[id].m_busy = 0; + return 0; +} + +int Cache::deleteDevice(const void *device) +{ + CacheMap::iterator it = m_cache.find(device); + CacheEntry *entry; + + if (it == m_cache.end()) { + // device does not exist + return -1; + } + entry = it->second; + delete entry; + m_cache.erase(it); + return 0; +} + +void Cache::clearCacheFrom(const void *device, CacheTS timestamp) +{ + CacheMap::iterator it = (device) ? m_cache.find(device) : m_cache.begin(); + CacheEntry *entry; + CacheChannel *channel; + CacheBuffer *buffer, *nextBuffer, *prevBuffer; + CacheItem *item, *prevItem, *nextItem; + unsigned int positionW, block; + + while (it != m_cache.end()) { + entry = it->second; + for (unsigned int ch=0; ch<entry->m_count; ch++) { + channel = &entry->m_channelArray[ch]; + if (channel->m_busy) { + item = channel->findItemOrLater(timestamp, &buffer); + if (item ) { + if (!buffer) { + // this is possible if we return the special timestamp=0 item, delete all buffers + channel->clear(); + } else { + // this item and all later items will be removed, clear any later buffer + while ((nextBuffer = buffer->m_next) != NULL) { + buffer->m_next = nextBuffer->m_next; + free(nextBuffer); + } + positionW = CACHE_ITEM_POSITIONW(buffer,item); + if (positionW == 0) { + // this item is the first one of the buffer, remove the buffer completely + // first find the buffer just before it + nextBuffer = channel->m_firstBuffer; + prevBuffer = NULL; + while (nextBuffer != buffer) { + prevBuffer = nextBuffer; + nextBuffer = nextBuffer->m_next; + // we must quit this loop before reaching the end of the list + assert(nextBuffer); + } + free(buffer); + buffer = prevBuffer; + if (buffer == NULL) + // this was also the first buffer + channel->m_firstBuffer = NULL; + } else { + // removing this item means finding the previous item to make it the last one + block = positionW>>channel->m_positionToBlockShiftW; + if (block == 0) { + // start from first item, we know it is not our item because positionW > 0 + prevItem = &buffer->m_firstItem; + } else { + // no need to check the current block, it will point to our item or a later one + // but the previous block will be a good start for sure. + block--; + prevItem = CACHE_BLOCK_ITEM_ADDR(channel,buffer,block); + } + while ((nextItem = CACHE_NEXT_ITEM(prevItem)) < item) + prevItem = nextItem; + // we must have found our item + assert(nextItem==item); + // now set the buffer + buffer->m_lastItemPositionW = CACHE_ITEM_POSITIONW(buffer,prevItem); + buffer->m_firstFreePositionW = positionW; + buffer->m_lastTimestamp = buffer->m_firstTimestamp + prevItem->m_timeOffset; + block = buffer->m_lastItemPositionW>>channel->m_positionToBlockShiftW; + buffer->lookup[block].m_offsetW = buffer->m_lastItemPositionW&channel->m_positionToOffsetMaskW; + buffer->lookup[block].m_timeOffset = prevItem->m_timeOffset; + } + // set the channel + channel->m_lastBuffer = buffer; + if (buffer) { + channel->m_lastTimestamp = buffer->m_lastTimestamp; + channel->m_lastItemPositionW = buffer->m_lastItemPositionW; + } + } + } + } + } + if (device) + break; + ++it; + } +} + +void *Cache::addCacheItem(const void *device, int id, unsigned int timestamp, void *data, unsigned int length) +{ + CacheMap::iterator it = m_cache.find(device); + CacheEntry *entry; + CacheChannel *channel; + CacheBuffer *buffer, *next; + CacheItem *item; + unsigned int positionW, sizeW, block; + + if (it == m_cache.end()) { + // device does not exist + return NULL; + } + entry = it->second; + if (id < 0 || id >= (int) entry->m_count || !entry->m_channelArray[id].m_busy) + return NULL; + channel = &entry->m_channelArray[id]; + if (length > channel->m_maxItemSizeB) + return NULL; + if (timestamp == 0) { + // initial item, delete all buffers + channel->clear(); + // and create initial item + item = NULL; + // we will allocate the memory, which is always pointer aligned => compute size + // with NULL will give same result. + sizeW = CACHE_ITEM_SIZEW(item,length); + item = (CacheItem*)calloc(sizeW, 4); + item->m_sizeW = sizeW; + channel->initItem = item; + } else { + if (!channel->m_lastBuffer) { + // no item in buffer, insert item at first position of first buffer + positionW = 0; + if ((buffer = channel->m_firstBuffer) == NULL) { + buffer = channel->allocBuffer(); + channel->m_firstBuffer = buffer; + } + } else if (timestamp > channel->m_lastTimestamp) { + // this is the normal case: we are writing past lastest timestamp + buffer = channel->m_lastBuffer; + positionW = buffer->m_firstFreePositionW; + } else if (timestamp == channel->m_lastTimestamp) { + // common case, rewriting the last timestamp, just reuse the last position + buffer = channel->m_lastBuffer; + positionW = channel->m_lastItemPositionW; + } else { + // general case, write in the middle of the buffer, locate the timestamp + // (or the timestamp just after), clear this item and all future items, + // and write at that position + item = channel->findItemOrLater(timestamp, &buffer); + if (item == NULL) { + // this should not happen + return NULL; + } + // this item will become the last one of this channel, clear any later buffer + while ((next = buffer->m_next) != NULL) { + buffer->m_next = next->m_next; + free(next); + } + // no need to update the buffer, this will be done when the item is written + positionW = CACHE_ITEM_POSITIONW(buffer,item); + } + item = CACHE_ITEM_ADDR(buffer,positionW); + sizeW = CACHE_ITEM_SIZEW(item,length); + // we have positionW pointing where we can put the item + // before we do that we have to check if we can: + // - enough room + // - timestamp not too late + if ((positionW+sizeW > channel->m_bufferSizeW) || + (positionW > 0 && timestamp >= buffer->m_firstTimestamp+0x10000)) { + // we must allocate a new buffer to store this item + // but before we must make sure that the current buffer is consistent + if (positionW != buffer->m_firstFreePositionW) { + // This means that we were trying to write in the middle of the buffer. + // We must set the buffer right with positionW being the last position + // and find the item before positionW to make it the last. + block = positionW>>channel->m_positionToBlockShiftW; + CacheItem *previousItem, *nextItem; + if (block == 0) { + // start from first item, we know it is not our item because positionW > 0 + previousItem = &buffer->m_firstItem; + } else { + // no need to check the current block, it will point to our item or a later one + // but the previous block will be a good start for sure. + block--; + previousItem = CACHE_BLOCK_ITEM_ADDR(channel,buffer,block); + } + while ((nextItem = CACHE_NEXT_ITEM(previousItem)) < item) + previousItem = nextItem; + // we must have found our item + assert(nextItem==item); + // now set the buffer + buffer->m_lastItemPositionW = CACHE_ITEM_POSITIONW(buffer,previousItem); + buffer->m_firstFreePositionW = positionW; + buffer->m_lastTimestamp = buffer->m_firstTimestamp + previousItem->m_timeOffset; + block = buffer->m_lastItemPositionW>>channel->m_positionToBlockShiftW; + buffer->lookup[block].m_offsetW = buffer->m_lastItemPositionW&channel->m_positionToOffsetMaskW; + buffer->lookup[block].m_timeOffset = previousItem->m_timeOffset; + // and also the channel, just in case + channel->m_lastBuffer = buffer; + channel->m_lastTimestamp = buffer->m_lastTimestamp; + channel->m_lastItemPositionW = buffer->m_lastItemPositionW; + } + // now allocate a new buffer + buffer->m_next = channel->allocBuffer(); + if (buffer->m_next == NULL) + return NULL; + buffer = buffer->m_next; + positionW = 0; + item = &buffer->m_firstItem; + sizeW = CACHE_ITEM_SIZEW(item,length); + } + // all check passed, ready to write the item + item->m_sizeW = sizeW; + if (positionW == 0) { + item->m_timeOffset = 0; + buffer->m_firstTimestamp = timestamp; + } else { + item->m_timeOffset = (unsigned short)(timestamp-buffer->m_firstTimestamp); + } + buffer->m_lastItemPositionW = positionW; + buffer->m_firstFreePositionW = positionW+sizeW; + buffer->m_lastTimestamp = timestamp; + block = positionW>>channel->m_positionToBlockShiftW; + buffer->lookup[block].m_offsetW = positionW&channel->m_positionToOffsetMaskW; + buffer->lookup[block].m_timeOffset = item->m_timeOffset; + buffer->m_lastItemPositionW = CACHE_ITEM_POSITIONW(buffer,item); + buffer->m_firstFreePositionW = buffer->m_lastItemPositionW+item->m_sizeW; + channel->m_lastBuffer = buffer; + channel->m_lastItemPositionW = positionW; + channel->m_lastTimestamp = timestamp; + } + // now copy the item + void *itemData = CACHE_ITEM_DATA_POINTER(item); + if (data) + memcpy(itemData, data, length); + return itemData; +} + +const void *Cache::getPreviousCacheItem(const void *device, int id, unsigned int *timestamp) +{ + CacheMap::iterator it; + CacheEntry *entry; + CacheChannel *channel; + CacheBuffer *buffer; + CacheItem *item; + + if (device) { + it = m_cache.find(device); + } else { + it = m_cache.begin(); + } + if (it == m_cache.end()) { + // device does not exist + return NULL; + } + entry = it->second; + if (id < 0 || id >= (int) entry->m_count || !entry->m_channelArray[id].m_busy) + return NULL; + channel = &entry->m_channelArray[id]; + if ((item = channel->findItemEarlier(*timestamp,&buffer)) == NULL) + return NULL; + *timestamp = (buffer) ? buffer->m_firstTimestamp+item->m_timeOffset : 0; + return CACHE_ITEM_DATA_POINTER(item); +} + +const CacheItem *Cache::getCurrentCacheItemInternal(const void *device, int id, CacheTS timestamp) +{ + CacheMap::iterator it = m_cache.find(device); + CacheEntry *entry; + CacheChannel *channel; + CacheBuffer *buffer; + CacheItem *item; + + if (it == m_cache.end()) { + // device does not exist + return NULL; + } + entry = it->second; + if (id < 0 || id >= (int) entry->m_count || !entry->m_channelArray[id].m_busy) + return NULL; + channel = &entry->m_channelArray[id]; + if ((item = channel->findItemOrLater(timestamp,&buffer)) == NULL) + return NULL; + if (buffer && buffer->m_firstTimestamp+item->m_timeOffset != timestamp) + return NULL; + return item; +} + +const void *Cache::getCurrentCacheItem(const void *device, int channel, unsigned int timestamp) +{ + const CacheItem *item = getCurrentCacheItemInternal(device, channel, timestamp); + return (item) ? CACHE_ITEM_DATA_POINTER(item) : NULL; +} + +double *Cache::addCacheVectorIfDifferent(const void *device, int channel, CacheTS timestamp, double *newdata, unsigned int length, double threshold) +{ + const CacheItem *item = getCurrentCacheItemInternal(device, channel, timestamp); + unsigned int sizeW = CACHE_ITEM_SIZEW(item,length*sizeof(double)); + if (!item || item->m_sizeW != sizeW) + return (double*)addCacheItem(device, channel, timestamp, newdata, length*sizeof(double)); + double *olddata = (double*)CACHE_ITEM_DATA_POINTER(item); + if (!length) + return olddata; + double *ref = olddata; + double *v = newdata; + unsigned int i; + for (i=length; i>0; --i) { + if (fabs(*v-*ref) > threshold) + break; + *ref++ = *v++; + } + if (i) + olddata = (double*)addCacheItem(device, channel, timestamp, newdata, length*sizeof(double)); + return olddata; +} + +} diff --git a/intern/itasc/Cache.hpp b/intern/itasc/Cache.hpp new file mode 100644 index 00000000000..64707782e6f --- /dev/null +++ b/intern/itasc/Cache.hpp @@ -0,0 +1,227 @@ +/* $Id: Cache.hpp 21152 2009-06-25 11:57:19Z ben2610 $ + * Cache.hpp + * + * Created on: Feb 24, 2009 + * Author: benoit tbolsee + */ + +#ifndef CACHE_HPP_ +#define CACHE_HPP_ + +#include <map> + +namespace iTaSC { + +#define CACHE_LOOKUP_TABLE_SIZE 128 +#define CACHE_DEFAULT_BUFFER_SIZE 32768 +#define CACHE_CHANNEL_EXTEND_SIZE 10 +#define CACHE_MAX_ITEM_SIZE 0x3FFF0 + +/* macro to get the alignement gap after an item header */ +#define CACHE_ITEM_GAPB(item) (unsigned int)(((size_t)item+sizeof(CacheItem))&(sizeof(void*)-1)) +/* macro to get item data position, item=CacheItem pointer */ +#define CACHE_ITEM_DATA_POINTER(item) (void*)((unsigned char*)item+sizeof(CacheItem)+CACHE_ITEM_GAPB(item)) +/* macro to get item size in 32bit words from item address and length, item=CacheItem pointer */ +#define CACHE_ITEM_SIZEW(item,length) (unsigned int)((sizeof(CacheItem)+CACHE_ITEM_GAPB(item)+(((length)+3)&~0x3))>>2) +/* macto to move from one item to the next, item=CacheItem pointer, updated by the macro */ +#define CACHE_NEXT_ITEM(item) ((item)+(item)->m_sizeW) +#define CACHE_BLOCK_ITEM_ADDR(chan,buf,block) (&(buf)->m_firstItem+(((unsigned int)(block)<<chan->m_positionToBlockShiftW)+(buf)->lookup[block].m_offsetW)) +#define CACHE_ITEM_ADDR(buf,pos) (&(buf)->m_firstItem+(pos)) +#define CACHE_ITEM_POSITIONW(buf,item) (unsigned int)(item-&buf->m_firstItem) + +typedef unsigned int CacheTS; + +struct Timestamp +{ + double realTimestamp; + double realTimestep; + CacheTS cacheTimestamp; + unsigned int numstep:8; + unsigned int substep:1; + unsigned int reiterate:1; + unsigned int cache:1; + unsigned int update:1; + unsigned int interpolate:1; + unsigned int dummy:19; + + Timestamp() { memset(this, 0, sizeof(Timestamp)); } +}; + +/* utility function to return second timestamp to millisecond */ +inline void setCacheTimestamp(Timestamp& timestamp) +{ + if (timestamp.realTimestamp < 0.0 || timestamp.realTimestamp > 4294967.295) + timestamp.cacheTimestamp = 0; + else + timestamp.cacheTimestamp = (CacheTS)(timestamp.realTimestamp*1000.0+0.5); +} + + +/* +class Cache: +Top level class, only one instance of this class should exists. +A device (=constraint, object) uses this class to create a cache entry for its data. +A cache entry is divided into cache channels, each providing a separate buffer for cache items. +The cache channels must be declared by the devices before they can be used. +The device must specify the largest cache item (limited to 256Kb) so that the cache +buffer can be organized optimally. +Cache channels are identified by small number (starting from 0) allocated by the cache system. +Cache items are inserted into cache channels ordered by timestamp. Writing is always done +at the end of the cache buffer: writing an item automatically clears all items with +higher timestamp. +A cache item is an array of bytes provided by the device; the format of the cache item is left +to the device. +The device can retrieve a cache item associated with a certain timestamp. The cache system +returns a pointer that points directly in the cache buffer to avoid unnecessary copy. +The pointer is guaranteed to be pointer aligned so that direct mapping to C structure is possible +(=32 bit aligned on 32 systems and 64 bit aligned on 64 bits system). + +Timestamp = rounded time in millisecond. +*/ + +struct CacheEntry; +struct CacheBuffer; +struct CacheItem; +struct CacheChannel; + +class Cache +{ +private: + /* map between device and cache entry. + Dynamically updated when more devices create cache channels */ + typedef std::map<const void *, struct CacheEntry*> CacheMap; + CacheMap m_cache; + const CacheItem *getCurrentCacheItemInternal(const void *device, int channel, CacheTS timestamp); + +public: + Cache(); + ~Cache(); + /* add a cache channel, maxItemSize must be < 256k. + name : channel name, truncated at 31 characters + msxItemSize : maximum size of item in bytes, items of larger size will be rejected + return value >= 0: channel id, -1: error */ + int addChannel(const void *device, const char *name, unsigned int maxItemSize); + + /* delete a cache channel (and all associated buffers and items) */ + int deleteChannel(const void *device, int channel); + /* delete all channels of a device and remove the device from the map */ + int deleteDevice(const void *device); + /* removes all cache items, leaving the special item at timestamp=0. + if device=NULL, apply to all devices. */ + void clearCacheFrom(const void *device, CacheTS timestamp); + + /* add a new cache item + channel: the cache channel (as returned by AddChannel + data, length: the cache item and length in bytes + If data is NULL, the memory is allocated in the cache but not writen + return: error: NULL, success: pointer to item in cache */ + void *addCacheItem(const void *device, int channel, CacheTS timestamp, void *data, unsigned int length); + + /* specialized function to add a vector of double in the cache + It will first check if a vector exist already in the cache for the same timestamp + and compared the cached vector with the new values. + If all values are within threshold, the vector is updated but the cache is not deleted + for the future timestamps. */ + double *addCacheVectorIfDifferent(const void *device, int channel, CacheTS timestamp, double *data, unsigned int length, double threshold); + + /* returns the cache item with timestamp that is just before the given timestamp. + returns the data pointer or NULL if there is no cache item before timestamp. + On return, timestamp is updated with the actual timestamp of the item being returned. + Note that the length of the item is not returned, it is up to the device to organize + the data so that length can be retrieved from the data if needed. + Device can NULL, it will then just look the first channel available, useful to + test the status of the cache. */ + const void *getPreviousCacheItem(const void *device, int channel, CacheTS *timestamp); + + /* returns the cache item with the timestamp that is exactly equal to the given timestamp + If there is no cache item for this timestamp, returns NULL.*/ + const void *getCurrentCacheItem(const void *device, int channel, CacheTS timestamp); + +}; + +/* the following structures are not internal use only, they should not be used directly */ + +struct CacheEntry +{ + CacheChannel *m_channelArray; // array of channels, automatically resized if more channels are created + unsigned int m_count; // number of channel in channelArray + CacheEntry() : m_channelArray(NULL), m_count(0) {} + ~CacheEntry(); +}; + +struct CacheChannel +{ + CacheItem* initItem; // item corresponding to timestamp=0 + struct CacheBuffer *m_firstBuffer; // first buffer of list + struct CacheBuffer *m_lastBuffer; // last buffer of list to which an item was written + char m_name[32]; // channel name + unsigned char m_busy; // =0 if channel is free, !=0 when channel is in use + unsigned char m_positionToBlockShiftW; // number of bits to shift a position in word to get the block number + unsigned short m_positionToOffsetMaskW; // bit mask to apply on a position in word to get offset in a block + unsigned int m_maxItemSizeB; // maximum item size in bytes + unsigned int m_bufferSizeW; // size of item buffer in word to allocate when a new buffer must be created + unsigned int m_blockSizeW; // block size in words of the lookup table + unsigned int m_lastTimestamp; // timestamp of the last item that was written + unsigned int m_lastItemPositionW; // position in words in lastBuffer of the last item written + void clear(); + CacheBuffer* allocBuffer(); + CacheItem* findItemOrLater(unsigned int timestamp, CacheBuffer **rBuffer); + CacheItem* findItemEarlier(unsigned int timestamp, CacheBuffer **rBuffer); + // Internal function: finds an item in a buffer that is < timeOffset + // timeOffset must be a valid offset for the buffer and the buffer must not be empty + // on return highBlock contains the block with items above or equal to timeOffset + CacheItem *_findBlock(CacheBuffer *buffer, unsigned short timeOffset, unsigned int *highBlock); +}; + +struct CacheBlock { + unsigned short m_timeOffset; // timestamp relative to m_firstTimestamp + unsigned short m_offsetW; // position in words of item relative to start of block +}; + +/* CacheItem is the header of each item in the buffer, must be 32bit + Items are always 32 bits aligned and size is the number of 32 bit words until the + next item header, including an eventual pre and post padding gap for pointer alignment */ +struct CacheItem +{ + unsigned short m_timeOffset; // timestamp relative to m_firstTimestamp + unsigned short m_sizeW; // size of item in 32 bit words + // item data follows header immediately or after a gap if position is not pointer aligned +}; + +// Buffer header +// Defined in a macro to avoid sizeof() potential problem. +// next for linked list. = NULL for last buffer +// m_firstTimestamp timestamp of first item in this buffer +// m_lastTimestamp timestamp of last item in this buffer +// m_lastTimestamp must be < m_firstTimestamp+65536 +// m_lastItemPositionW position in word of last item written +// m_firstFreePositionW position in word where a new item can be written, 0 if buffer is empty +// lookup lookup table for fast access to item by timestamp +// The buffer is divided in blocks of 2**n bytes with n chosen so that +// there are no more than CACHE_LOOKUP_TABLE_SIZE blocks and that each +// block will contain at least one item. +// Each element of the lookup table gives the timestamp and offset +// of the last cache item occupying (=starting in) the corresponding block. +#define CACHE_HEADER \ + struct CacheBuffer *m_next; \ + unsigned int m_firstTimestamp; \ + unsigned int m_lastTimestamp; \ + \ + unsigned int m_lastItemPositionW; \ + unsigned int m_firstFreePositionW;\ + struct CacheBlock lookup[CACHE_LOOKUP_TABLE_SIZE] + +struct CacheBufferHeader { + CACHE_HEADER; +}; +#define CACHE_BUFFER_HEADER_SIZE (sizeof(struct CacheBufferHeader)) +struct CacheBuffer +{ + CACHE_HEADER; + struct CacheItem m_firstItem; // the address of this field marks the start of the buffer +}; + + +} + +#endif /* CACHE_HPP_ */ diff --git a/intern/itasc/ConstraintSet.cpp b/intern/itasc/ConstraintSet.cpp new file mode 100644 index 00000000000..a38db445ea2 --- /dev/null +++ b/intern/itasc/ConstraintSet.cpp @@ -0,0 +1,170 @@ +/* $Id$ + * ConstraintSet.cpp + * + * Created on: Jan 5, 2009 + * Author: rubensmits + */ + +#include "ConstraintSet.hpp" +#include "kdl/utilities/svd_eigen_HH.hpp" + +namespace iTaSC { + +ConstraintSet::ConstraintSet(unsigned int _nc,double accuracy,unsigned int maximum_iterations): + m_nc(_nc), + m_Cf(e_zero_matrix(m_nc,6)), + m_Wy(e_scalar_vector(m_nc,1.0)), + m_y(m_nc),m_ydot(e_zero_vector(m_nc)),m_chi(e_zero_vector(6)), + m_S(6),m_temp(6),m_tdelta(6), + m_Jf(e_identity_matrix(6,6)), + m_U(e_identity_matrix(6,6)),m_V(e_identity_matrix(6,6)),m_B(e_zero_matrix(6,6)), + m_Jf_inv(e_zero_matrix(6,6)), + m_internalPose(F_identity), m_externalPose(F_identity), + m_constraintCallback(NULL), m_constraintParam(NULL), + m_toggle(false),m_substep(false), + m_threshold(accuracy),m_maxIter(maximum_iterations) +{ + m_maxDeltaChi = e_scalar(0.52); +} + +ConstraintSet::ConstraintSet(): + m_nc(0), + m_internalPose(F_identity), m_externalPose(F_identity), + m_constraintCallback(NULL), m_constraintParam(NULL), + m_toggle(false),m_substep(false), + m_threshold(0.0),m_maxIter(0) +{ + m_maxDeltaChi = e_scalar(0.52); +} + +void ConstraintSet::reset(unsigned int _nc,double accuracy,unsigned int maximum_iterations) +{ + m_nc = _nc; + m_Jf = e_identity_matrix(6,6); + m_Cf = e_zero_matrix(m_nc,6); + m_U = e_identity_matrix(6,6); + m_V = e_identity_matrix(6,6); + m_B = e_zero_matrix(6,6); + m_Jf_inv = e_zero_matrix(6,6), + m_Wy = e_scalar_vector(m_nc,1.0), + m_chi = e_zero_vector(6); + m_chidot = e_zero_vector(6); + m_y = e_zero_vector(m_nc); + m_ydot = e_zero_vector(m_nc); + m_S = e_zero_vector(6); + m_temp = e_zero_vector(6); + m_tdelta = e_zero_vector(6); + m_threshold = accuracy; + m_maxIter = maximum_iterations; +} + +ConstraintSet::~ConstraintSet() { + +} + +void ConstraintSet::modelUpdate(Frame& _external_pose,const Timestamp& timestamp) +{ + m_chi+=m_chidot*timestamp.realTimestep; + m_externalPose = _external_pose; + + //update the internal pose and Jf + updateJacobian(); + //check if loop is already closed, if not update the pose and Jf + unsigned int iter=0; + while(iter<5&&!closeLoop()) + iter++; +} + +double ConstraintSet::getMaxTimestep(double& timestep) +{ + e_scalar maxChidot = m_chidot.cwise().abs().maxCoeff(); + if (timestep*maxChidot > m_maxDeltaChi) { + timestep = m_maxDeltaChi/maxChidot; + } + return timestep; +} + +bool ConstraintSet::initialise(Frame& init_pose){ + m_externalPose=init_pose; + // get current Jf + updateJacobian(); + + unsigned int iter=0; + while(iter<m_maxIter&&!closeLoop()){ + iter++; + } + if (iter<m_maxIter) + return true; + else + return false; +} + +bool ConstraintSet::setControlParameter(int id, ConstraintAction action, double data, double timestep) +{ + ConstraintValues values; + ConstraintSingleValue value; + values.values = &value; + values.number = 0; + values.action = action; + values.id = id; + value.action = action; + value.id = id; + switch (action) { + case ACT_NONE: + return true; + case ACT_VALUE: + value.yd = data; + values.number = 1; + break; + case ACT_VELOCITY: + value.yddot = data; + values.number = 1; + break; + case ACT_TOLERANCE: + values.tolerance = data; + break; + case ACT_FEEDBACK: + values.feedback = data; + break; + case ACT_ALPHA: + values.alpha = data; + break; + default: + assert(action==ACT_NONE); + } + return setControlParameters(&values, 1, timestep); +} + +bool ConstraintSet::closeLoop(){ + //Invert Jf + //TODO: svd_boost_Macie has problems if Jf contains zero-rows + //toggle=!toggle; + //svd_boost_Macie(Jf,U,S,V,B,temp,1e-3*threshold,toggle); + int ret = KDL::svd_eigen_HH(m_Jf,m_U,m_S,m_V,m_temp); + if(ret<0) + return false; + + // the reference point and frame of the jacobian is the base frame + // m_externalPose-m_internalPose is the twist to extend the end effector + // to get the required pose => change the reference point to the base frame + Twist twist_delta(diff(m_internalPose,m_externalPose)); + twist_delta=twist_delta.RefPoint(-m_internalPose.p); + for(unsigned int i=0;i<6;i++) + m_tdelta(i)=twist_delta(i); + //TODO: use damping in constraintset inversion? + for(unsigned int i=0;i<6;i++) + if(m_S(i)<m_threshold){ + m_B.row(i).setConstant(0.0); + }else + m_B.row(i) = m_U.col(i)/m_S(i); + + m_Jf_inv=(m_V*m_B).lazy(); + + m_chi+=(m_Jf_inv*m_tdelta).lazy(); + updateJacobian(); + // m_externalPose-m_internalPose in end effector frame + // this is just to compare the pose, a different formula would work too + return Equal(m_internalPose.Inverse()*m_externalPose,F_identity,m_threshold); + +} +} diff --git a/intern/itasc/ConstraintSet.hpp b/intern/itasc/ConstraintSet.hpp new file mode 100644 index 00000000000..12bb085c911 --- /dev/null +++ b/intern/itasc/ConstraintSet.hpp @@ -0,0 +1,115 @@ +/* $Id: ConstraintSet.hpp 20307 2009-05-20 20:39:18Z ben2610 $ + * ConstraintSet.hpp + * + * Created on: Jan 5, 2009 + * Author: rubensmits + */ + +#ifndef CONSTRAINTSET_HPP_ +#define CONSTRAINTSET_HPP_ + +#include "kdl/frames.hpp" +#include "eigen_types.hpp" +#include "Cache.hpp" +#include <vector> + +namespace iTaSC { + +enum ConstraintAction { + ACT_NONE= 0, + ACT_VALUE= 1, + ACT_VELOCITY= 2, + ACT_TOLERANCE= 4, + ACT_FEEDBACK= 8, + ACT_ALPHA= 16 +}; + +struct ConstraintSingleValue { + unsigned int id; // identifier of constraint value, depends on constraint + unsigned int action;// action performed, compbination of ACT_..., set on return + const double y; // actual constraint value + const double ydot; // actual constraint velocity + double yd; // current desired constraint value, changed on return + double yddot; // current desired constraint velocity, changed on return + ConstraintSingleValue(): id(0), action(0), y(0.0), ydot(0.0) {} +}; + +struct ConstraintValues { + unsigned int id; // identifier of group of constraint values, depend on constraint + unsigned short number; // number of constraints in list + unsigned short action; // action performed, ACT_..., set on return + double alpha; // constraint activation coefficient, should be [0..1] + double tolerance; // current desired tolerance on constraint, same unit than yd, changed on return + double feedback; // current desired feedback on error, in 1/sec, changed on return + struct ConstraintSingleValue* values; + ConstraintValues(): id(0), number(0), action(0), values(NULL) {} +}; + +class ConstraintSet; +typedef bool (*ConstraintCallback)(const Timestamp& timestamp, struct ConstraintValues* const _values, unsigned int _nvalues, void* _param); + +class ConstraintSet { +protected: + unsigned int m_nc; + e_scalar m_maxDeltaChi; + e_matrix m_Cf; + e_vector m_Wy,m_y,m_ydot; + e_vector6 m_chi,m_chidot,m_S,m_temp,m_tdelta; + e_matrix6 m_Jf,m_U,m_V,m_B,m_Jf_inv; + KDL::Frame m_internalPose,m_externalPose; + ConstraintCallback m_constraintCallback; + void* m_constraintParam; + void* m_poseParam; + bool m_toggle; + bool m_substep; + double m_threshold; + unsigned int m_maxIter; + + friend class Scene; + virtual void modelUpdate(KDL::Frame& _external_pose,const Timestamp& timestamp); + virtual void updateKinematics(const Timestamp& timestamp)=0; + virtual void pushCache(const Timestamp& timestamp)=0; + virtual void updateJacobian()=0; + virtual void updateControlOutput(const Timestamp& timestamp)=0; + virtual void initCache(Cache *_cache) = 0; + virtual bool initialise(KDL::Frame& init_pose); + virtual void reset(unsigned int nc,double accuracy,unsigned int maximum_iterations); + virtual bool closeLoop(); + virtual double getMaxTimestep(double& timestep); + + +public: + ConstraintSet(unsigned int nc,double accuracy,unsigned int maximum_iterations); + ConstraintSet(); + virtual ~ConstraintSet(); + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + virtual bool registerCallback(ConstraintCallback _function, void* _param) + { + m_constraintCallback = _function; + m_constraintParam = _param; + return true; + } + + virtual const e_vector& getControlOutput()const{return m_ydot;}; + virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues) = 0; + virtual bool setControlParameters(ConstraintValues* _values, unsigned int _nvalues, double timestep=0.0) = 0; + bool setControlParameter(int id, ConstraintAction action, double value, double timestep=0.0); + + virtual const e_matrix6& getJf() const{return m_Jf;}; + virtual const KDL::Frame& getPose() const{return m_internalPose;}; + virtual const e_matrix& getCf() const{return m_Cf;}; + + virtual const e_vector& getWy() const {return m_Wy;}; + virtual void setWy(const e_vector& Wy_in){m_Wy = Wy_in;}; + virtual void setJointVelocity(const e_vector chidot_in){m_chidot = chidot_in;}; + + virtual unsigned int getNrOfConstraints(){return m_nc;}; + void substep(bool _substep) {m_substep=_substep;} + bool substep() {return m_substep;} +}; + +} + +#endif /* CONSTRAINTSET_HPP_ */ diff --git a/intern/itasc/ControlledObject.cpp b/intern/itasc/ControlledObject.cpp new file mode 100644 index 00000000000..b987e176031 --- /dev/null +++ b/intern/itasc/ControlledObject.cpp @@ -0,0 +1,61 @@ +/* $Id$ + * ControlledObject.cpp + * + * Created on: Jan 5, 2009 + * Author: rubensmits + */ + +#include "ControlledObject.hpp" + + +namespace iTaSC { +ControlledObject::ControlledObject(): + Object(Controlled),m_nq(0),m_nc(0),m_nee(0) +{ + // max joint variable = 0.52 radian or 0.52 meter in one timestep + m_maxDeltaQ = e_scalar(0.52); +} + +void ControlledObject::initialize(unsigned int _nq,unsigned int _nc, unsigned int _nee) +{ + assert(_nee >= 1); + m_nq = _nq; + m_nc = _nc; + m_nee = _nee; + if (m_nq > 0) { + m_Wq = e_identity_matrix(m_nq,m_nq); + m_qdot = e_zero_vector(m_nq); + } + if (m_nc > 0) { + m_Wy = e_scalar_vector(m_nc,1.0); + m_ydot = e_zero_vector(m_nc); + } + if (m_nc > 0 && m_nq > 0) + m_Cq = e_zero_matrix(m_nc,m_nq); + // clear all Jacobian if any + m_JqArray.clear(); + // reserve one more to have a zero matrix handy + if (m_nq > 0) + m_JqArray.resize(m_nee+1, e_zero_matrix(6,m_nq)); +} + +ControlledObject::~ControlledObject() {} + + + +const e_matrix& ControlledObject::getJq(unsigned int ee) const +{ + assert(m_nq > 0); + return m_JqArray[(ee>m_nee)?m_nee:ee]; +} + +double ControlledObject::getMaxTimestep(double& timestep) +{ + e_scalar maxQdot = m_qdot.cwise().abs().maxCoeff(); + if (timestep*maxQdot > m_maxDeltaQ) { + timestep = m_maxDeltaQ/maxQdot; + } + return timestep; +} + +} diff --git a/intern/itasc/ControlledObject.hpp b/intern/itasc/ControlledObject.hpp new file mode 100644 index 00000000000..2370f6594ed --- /dev/null +++ b/intern/itasc/ControlledObject.hpp @@ -0,0 +1,70 @@ +/* $Id: ControlledObject.hpp 20853 2009-06-13 12:29:46Z ben2610 $ + * ControlledObject.hpp + * + * Created on: Jan 5, 2009 + * Author: rubensmits + */ + +#ifndef CONTROLLEDOBJECT_HPP_ +#define CONTROLLEDOBJECT_HPP_ + +#include "kdl/frames.hpp" +#include "eigen_types.hpp" + +#include "Object.hpp" +#include "ConstraintSet.hpp" +#include <vector> + +namespace iTaSC { + +#define CONSTRAINT_ID_ALL ((unsigned int)-1) + +class ControlledObject : public Object { +protected: + e_scalar m_maxDeltaQ; + unsigned int m_nq,m_nc,m_nee; + e_matrix m_Wq,m_Cq; + e_vector m_Wy,m_ydot,m_qdot; + std::vector<e_matrix> m_JqArray; +public: + ControlledObject(); + virtual ~ControlledObject(); + + class JointLockCallback { + public: + JointLockCallback() {} + virtual ~JointLockCallback() {} + + // lock a joint, no need to update output + virtual void lockJoint(unsigned int q_nr, unsigned int ndof) = 0; + // lock a joint and update output in view of reiteration + virtual void lockJoint(unsigned int q_nr, unsigned int ndof, double* qdot) = 0; + }; + + virtual void initialize(unsigned int _nq,unsigned int _nc, unsigned int _nee); + + // returns true when a joint has been locked via the callback and the solver must run again + virtual bool updateJoint(const Timestamp& timestamp, JointLockCallback& callback) = 0; + virtual void updateControlOutput(const Timestamp& timestamp)=0; + virtual void setJointVelocity(const e_vector qdot_in){m_qdot = qdot_in;}; + virtual double getMaxTimestep(double& timestep); + virtual bool setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, e_scalar value, double timestep=0.0)=0; + + virtual const e_vector& getControlOutput() const{return m_ydot;} + + virtual const e_matrix& getJq(unsigned int ee) const; + + virtual const e_matrix& getCq() const{return m_Cq;}; + + virtual e_matrix& getWq() {return m_Wq;}; + virtual void setWq(const e_matrix& Wq_in){m_Wq = Wq_in;}; + + virtual const e_vector& getWy() const {return m_Wy;}; + + virtual const unsigned int getNrOfCoordinates(){return m_nq;}; + virtual const unsigned int getNrOfConstraints(){return m_nc;}; +}; + +} + +#endif /* CONTROLLEDOBJECT_HPP_ */ diff --git a/intern/itasc/CopyPose.cpp b/intern/itasc/CopyPose.cpp new file mode 100644 index 00000000000..69722909ed1 --- /dev/null +++ b/intern/itasc/CopyPose.cpp @@ -0,0 +1,480 @@ +/* $Id$ + * CopyPose.cpp + * + * Created on: Mar 17, 2009 + * Author: benoit bolsee + */ + +#include "CopyPose.hpp" +#include "kdl/kinfam_io.hpp" +#include <math.h> +#include <string.h> + +namespace iTaSC +{ + +const unsigned int maxPoseCacheSize = (2*(3+3*2)); +CopyPose::CopyPose(unsigned int control_output, unsigned int dynamic_output, double armlength, double accuracy, unsigned int maximum_iterations): + ConstraintSet(), + m_cache(NULL), + m_poseCCh(-1),m_poseCTs(0) +{ + m_maxerror = armlength/2.0; + m_outputControl = (control_output & CTL_ALL); + unsigned int _nc = nBitsOn(m_outputControl); + if (!_nc) + return; + // reset the constraint set + reset(_nc, accuracy, maximum_iterations); + _nc = 0; + m_nvalues = 0; + int nrot = 0, npos = 0; + int nposCache = 0, nrotCache = 0; + m_outputDynamic = (dynamic_output & m_outputControl); + memset(m_values, 0, sizeof(m_values)); + memset(m_posData, 0, sizeof(m_posData)); + memset(m_rotData, 0, sizeof(m_rotData)); + memset(&m_rot, 0, sizeof(m_rot)); + memset(&m_pos, 0, sizeof(m_pos)); + if (m_outputControl & CTL_POSITION) { + m_pos.alpha = 1.0; + m_pos.K = 20.0; + m_pos.tolerance = 0.05; + m_values[m_nvalues].alpha = m_pos.alpha; + m_values[m_nvalues].feedback = m_pos.K; + m_values[m_nvalues].tolerance = m_pos.tolerance; + m_values[m_nvalues].id = ID_POSITION; + if (m_outputControl & CTL_POSITIONX) { + m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/; + m_Cf(_nc++,0)=1.0; + m_posData[npos++].id = ID_POSITIONX; + if (m_outputDynamic & CTL_POSITIONX) + nposCache++; + } + if (m_outputControl & CTL_POSITIONY) { + m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/; + m_Cf(_nc++,1)=1.0; + m_posData[npos++].id = ID_POSITIONY; + if (m_outputDynamic & CTL_POSITIONY) + nposCache++; + } + if (m_outputControl & CTL_POSITIONZ) { + m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/; + m_Cf(_nc++,2)=1.0; + m_posData[npos++].id = ID_POSITIONZ; + if (m_outputDynamic & CTL_POSITIONZ) + nposCache++; + } + m_values[m_nvalues].number = npos; + m_values[m_nvalues++].values = m_posData; + m_pos.firsty = 0; + m_pos.ny = npos; + } + if (m_outputControl & CTL_ROTATION) { + m_rot.alpha = 1.0; + m_rot.K = 20.0; + m_rot.tolerance = 0.05; + m_values[m_nvalues].alpha = m_rot.alpha; + m_values[m_nvalues].feedback = m_rot.K; + m_values[m_nvalues].tolerance = m_rot.tolerance; + m_values[m_nvalues].id = ID_ROTATION; + if (m_outputControl & CTL_ROTATIONX) { + m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/; + m_Cf(_nc++,3)=1.0; + m_rotData[nrot++].id = ID_ROTATIONX; + if (m_outputDynamic & CTL_ROTATIONX) + nrotCache++; + } + if (m_outputControl & CTL_ROTATIONY) { + m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/; + m_Cf(_nc++,4)=1.0; + m_rotData[nrot++].id = ID_ROTATIONY; + if (m_outputDynamic & CTL_ROTATIONY) + nrotCache++; + } + if (m_outputControl & CTL_ROTATIONZ) { + m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/; + m_Cf(_nc++,5)=1.0; + m_rotData[nrot++].id = ID_ROTATIONZ; + if (m_outputDynamic & CTL_ROTATIONZ) + nrotCache++; + } + m_values[m_nvalues].number = nrot; + m_values[m_nvalues++].values = m_rotData; + m_rot.firsty = npos; + m_rot.ny = nrot; + } + assert(_nc == m_nc); + m_Jf=e_identity_matrix(6,6); + m_poseCacheSize = ((nrotCache)?(3+nrotCache*2):0)+((nposCache)?(3+nposCache*2):0); +} + +CopyPose::~CopyPose() +{ +} + +bool CopyPose::initialise(Frame& init_pose) +{ + m_externalPose = m_internalPose = init_pose; + updateJacobian(); + return true; +} + +void CopyPose::modelUpdate(Frame& _external_pose,const Timestamp& timestamp) +{ + m_internalPose = m_externalPose = _external_pose; + updateJacobian(); +} + +void CopyPose::initCache(Cache *_cache) +{ + m_cache = _cache; + m_poseCCh = -1; + if (m_cache) { + // create one channel for the coordinates + m_poseCCh = m_cache->addChannel(this, "Xf", m_poseCacheSize*sizeof(double)); + // don't save initial value, it will be recomputed from external pose + //pushPose(0); + } +} + +double* CopyPose::pushValues(double* item, ControlState* _state, unsigned int mask) +{ + ControlState::ControlValue* _yval; + int i; + + *item++ = _state->alpha; + *item++ = _state->K; + *item++ = _state->tolerance; + + for (i=0, _yval=_state->output; i<_state->ny; mask<<=1) { + if (m_outputControl & mask) { + if (m_outputDynamic & mask) { + *item++ = _yval->yd; + *item++ = _yval->yddot; + } + _yval++; + i++; + } + } + return item; +} + +void CopyPose::pushPose(CacheTS timestamp) +{ + if (m_poseCCh >= 0) { + if (m_poseCacheSize) { + double buf[maxPoseCacheSize]; + double *item = buf; + if (m_outputDynamic & CTL_POSITION) + item = pushValues(item, &m_pos, CTL_POSITIONX); + if (m_outputDynamic & CTL_ROTATION) + item = pushValues(item, &m_rot, CTL_ROTATIONX); + m_cache->addCacheVectorIfDifferent(this, m_poseCCh, timestamp, buf, m_poseCacheSize, KDL::epsilon); + } else + m_cache->addCacheVectorIfDifferent(this, m_poseCCh, timestamp, NULL, 0, KDL::epsilon); + m_poseCTs = timestamp; + } +} + +double* CopyPose::restoreValues(double* item, ConstraintValues* _values, ControlState* _state, unsigned int mask) +{ + ConstraintSingleValue* _data; + ControlState::ControlValue* _yval; + int i, j; + + _values->alpha = _state->alpha = *item++; + _values->feedback = _state->K = *item++; + _values->tolerance = _state->tolerance = *item++; + + for (i=_state->firsty, j=i+_state->ny, _yval=_state->output, _data=_values->values; i<j; mask<<=1) { + if (m_outputControl & mask) { + m_Wy(i) = _state->alpha/*/(_state->tolerance*_state->K)*/; + if (m_outputDynamic & mask) { + _data->yd = _yval->yd = *item++; + _data->yddot = _yval->yddot = *item++; + } + _data++; + _yval++; + i++; + } + } + return item; +} + +bool CopyPose::popPose(CacheTS timestamp) +{ + bool found = false; + if (m_poseCCh >= 0) { + double *item = (double*)m_cache->getPreviousCacheItem(this, m_poseCCh, ×tamp); + if (item) { + found = true; + if (timestamp != m_poseCTs) { + int i=0; + if (m_outputControl & CTL_POSITION) { + if (m_outputDynamic & CTL_POSITION) { + item = restoreValues(item, &m_values[i], &m_pos, CTL_POSITIONX); + } + i++; + } + if (m_outputControl & CTL_ROTATION) { + if (m_outputDynamic & CTL_ROTATION) { + item = restoreValues(item, &m_values[i], &m_rot, CTL_ROTATIONX); + } + i++; + } + m_poseCTs = timestamp; + item = NULL; + } + } + } + return found; +} + +void CopyPose::interpolateOutput(ControlState* _state, unsigned int mask, const Timestamp& timestamp) +{ + ControlState::ControlValue* _yval; + int i; + + for (i=0, _yval=_state->output; i<_state->ny; mask <<= 1) { + if (m_outputControl & mask) { + if (m_outputDynamic & mask) { + if (timestamp.substep && timestamp.interpolate) { + _yval->yd += _yval->yddot*timestamp.realTimestep; + } else { + _yval->yd = _yval->nextyd; + _yval->yddot = _yval->nextyddot; + } + } + i++; + _yval++; + } + } +} + +void CopyPose::pushCache(const Timestamp& timestamp) +{ + if (!timestamp.substep && timestamp.cache) { + pushPose(timestamp.cacheTimestamp); + } +} + +void CopyPose::updateKinematics(const Timestamp& timestamp) +{ + if (timestamp.interpolate) { + if (m_outputDynamic & CTL_POSITION) + interpolateOutput(&m_pos, CTL_POSITIONX, timestamp); + if (m_outputDynamic & CTL_ROTATION) + interpolateOutput(&m_rot, CTL_ROTATIONX, timestamp); + } + pushCache(timestamp); +} + +void CopyPose::updateJacobian() +{ + //Jacobian is always identity at the start of the constraint chain + //instead of going through complicated jacobian operation, implemented direct formula + //m_Jf(1,3) = m_internalPose.p.z(); + //m_Jf(2,3) = -m_internalPose.p.y(); + //m_Jf(0,4) = -m_internalPose.p.z(); + //m_Jf(2,4) = m_internalPose.p.x(); + //m_Jf(0,5) = m_internalPose.p.y(); + //m_Jf(1,5) = -m_internalPose.p.x(); +} + +void CopyPose::updateState(ConstraintValues* _values, ControlState* _state, unsigned int mask, double timestep) +{ + unsigned int id = (mask == CTL_ROTATIONX) ? ID_ROTATIONX : ID_POSITIONX; + ControlState::ControlValue* _yval; + ConstraintSingleValue* _data; + int i, j, k; + int action = 0; + + if ((_values->action & ACT_ALPHA) && _values->alpha >= 0.0) { + _state->alpha = _values->alpha; + action |= ACT_ALPHA; + } + if ((_values->action & ACT_TOLERANCE) && _values->tolerance > KDL::epsilon) { + _state->tolerance = _values->tolerance; + action |= ACT_TOLERANCE; + } + if ((_values->action & ACT_FEEDBACK) && _values->feedback > KDL::epsilon) { + _state->K = _values->feedback; + action |= ACT_FEEDBACK; + } + for (i=_state->firsty, j=_state->firsty+_state->ny, _yval=_state->output; i<j; mask <<= 1, id++) { + if (m_outputControl & mask) { + if (action) + m_Wy(i) = _state->alpha/*/(_state->tolerance*_state->K)*/; + // check if this controlled output is provided + for (k=0, _data=_values->values; k<_values->number; k++, _data++) { + if (_data->id == id) { + switch (_data->action & (ACT_VALUE|ACT_VELOCITY)) { + case 0: + // no indication, keep current values + break; + case ACT_VELOCITY: + // only the velocity is given estimate the new value by integration + _data->yd = _yval->yd+_data->yddot*timestep; + // walkthrough + case ACT_VALUE: + _yval->nextyd = _data->yd; + // if the user sets the value, we assume future velocity is zero + // (until the user changes the value again) + _yval->nextyddot = (_data->action & ACT_VALUE) ? 0.0 : _data->yddot; + if (timestep>0.0) { + _yval->yddot = (_data->yd-_yval->yd)/timestep; + } else { + // allow the user to change target instantenously when this function + // if called from setControlParameter with timestep = 0 + _yval->yd = _yval->nextyd; + _yval->yddot = _yval->nextyddot; + } + break; + case (ACT_VALUE|ACT_VELOCITY): + // the user should not set the value and velocity at the same time. + // In this case, we will assume that he wants to set the future value + // and we compute the current value to match the velocity + _yval->yd = _data->yd - _data->yddot*timestep; + _yval->nextyd = _data->yd; + _yval->nextyddot = _data->yddot; + if (timestep>0.0) { + _yval->yddot = (_data->yd-_yval->yd)/timestep; + } else { + _yval->yd = _yval->nextyd; + _yval->yddot = _yval->nextyddot; + } + break; + } + } + } + _yval++; + i++; + } + } +} + + +bool CopyPose::setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep) +{ + while (_nvalues > 0) { + if (_values->id >= ID_POSITION && _values->id <= ID_POSITIONZ && (m_outputControl & CTL_POSITION)) { + updateState(_values, &m_pos, CTL_POSITIONX, timestep); + } + if (_values->id >= ID_ROTATION && _values->id <= ID_ROTATIONZ && (m_outputControl & CTL_ROTATION)) { + updateState(_values, &m_rot, CTL_ROTATIONX, timestep); + } + _values++; + _nvalues--; + } + return true; +} + +void CopyPose::updateValues(Vector& vel, ConstraintValues* _values, ControlState* _state, unsigned int mask) +{ + ConstraintSingleValue* _data; + ControlState::ControlValue* _yval; + int i, j; + + _values->action = 0; + + for (i=_state->firsty, j=0, _yval=_state->output, _data=_values->values; j<3; j++, mask<<=1) { + if (m_outputControl & mask) { + *(double*)&_data->y = vel(j); + *(double*)&_data->ydot = m_ydot(i); + _data->yd = _yval->yd; + _data->yddot = _yval->yddot; + _data->action = 0; + i++; + _data++; + _yval++; + } + } +} + +void CopyPose::updateOutput(Vector& vel, ControlState* _state, unsigned int mask) +{ + ControlState::ControlValue* _yval; + int i, j; + double coef=1.0; + if (mask & CTL_POSITION) { + // put a limit on position error + double len=0.0; + for (j=0, _yval=_state->output; j<3; j++) { + if (m_outputControl & (mask<<j)) { + len += KDL::sqr(_yval->yd-vel(j)); + _yval++; + } + } + len = KDL::sqrt(len); + if (len > m_maxerror) + coef = m_maxerror/len; + } + for (i=_state->firsty, j=0, _yval=_state->output; j<3; j++) { + if (m_outputControl & (mask<<j)) { + m_ydot(i)=_yval->yddot+_state->K*coef*(_yval->yd-vel(j)); + _yval++; + i++; + } + } +} + +void CopyPose::updateControlOutput(const Timestamp& timestamp) +{ + //IMO this should be done, no idea if it is enough (wrt Distance impl) + Twist y = diff(F_identity, m_internalPose); + bool found = true; + if (!timestamp.substep) { + if (!timestamp.reiterate) { + found = popPose(timestamp.cacheTimestamp); + } + } + if (m_constraintCallback && (m_substep || (!timestamp.reiterate && !timestamp.substep))) { + // initialize first callback the application to get the current values + int i=0; + if (m_outputControl & CTL_POSITION) { + updateValues(y.vel, &m_values[i++], &m_pos, CTL_POSITIONX); + } + if (m_outputControl & CTL_ROTATION) { + updateValues(y.rot, &m_values[i++], &m_rot, CTL_ROTATIONX); + } + if ((*m_constraintCallback)(timestamp, m_values, m_nvalues, m_constraintParam)) { + setControlParameters(m_values, m_nvalues, (found && timestamp.interpolate)?timestamp.realTimestep:0.0); + } + } + if (m_outputControl & CTL_POSITION) { + updateOutput(y.vel, &m_pos, CTL_POSITIONX); + } + if (m_outputControl & CTL_ROTATION) { + updateOutput(y.rot, &m_rot, CTL_ROTATIONX); + } +} + +const ConstraintValues* CopyPose::getControlParameters(unsigned int* _nvalues) +{ + Twist y = diff(m_internalPose,F_identity); + int i=0; + if (m_outputControl & CTL_POSITION) { + updateValues(y.vel, &m_values[i++], &m_pos, CTL_POSITIONX); + } + if (m_outputControl & CTL_ROTATION) { + updateValues(y.rot, &m_values[i++], &m_rot, CTL_ROTATIONX); + } + if (_nvalues) + *_nvalues=m_nvalues; + return m_values; +} + +double CopyPose::getMaxTimestep(double& timestep) +{ + // CopyPose should not have any limit on linear velocity: + // in case the target is out of reach, this can be very high. + // We will simply limit on rotation + e_scalar maxChidot = m_chidot.block(3,0,3,1).cwise().abs().maxCoeff(); + if (timestep*maxChidot > m_maxDeltaChi) { + timestep = m_maxDeltaChi/maxChidot; + } + return timestep; +} + +} diff --git a/intern/itasc/CopyPose.hpp b/intern/itasc/CopyPose.hpp new file mode 100644 index 00000000000..3a3f60a9f37 --- /dev/null +++ b/intern/itasc/CopyPose.hpp @@ -0,0 +1,99 @@ +/* $Id: CopyPose.hpp 20622 2009-06-04 12:47:59Z ben2610 $ + * CopyPose.h + * + * Created on: Mar 17, 2009 + * Author: benoit bolsee + */ + +#ifndef COPYPOSE_H_ +#define COPYPOSE_H_ + +#include "ConstraintSet.hpp" +namespace iTaSC{ + +using namespace KDL; + +class CopyPose: public iTaSC::ConstraintSet +{ +protected: + virtual void updateKinematics(const Timestamp& timestamp); + virtual void pushCache(const Timestamp& timestamp); + virtual void updateJacobian(); + virtual bool initialise(Frame& init_pose); + virtual void initCache(Cache *_cache); + virtual void updateControlOutput(const Timestamp& timestamp); + virtual void modelUpdate(Frame& _external_pose,const Timestamp& timestamp); + virtual double getMaxTimestep(double& timestep); + +public: + enum ID { // constraint ID in callback and setControlParameter + ID_POSITION=0, + ID_POSITIONX=1, + ID_POSITIONY=2, + ID_POSITIONZ=3, + ID_ROTATION=4, + ID_ROTATIONX=5, + ID_ROTATIONY=6, + ID_ROTATIONZ=7, + }; + enum CTL { // control ID in constructor to specify which output is constrainted + CTL_NONE=0x00, + CTL_POSITIONX=0x01, // the bit order is important: it matches the y output order + CTL_POSITIONY=0x02, + CTL_POSITIONZ=0x04, + CTL_POSITION=0x07, + CTL_ROTATIONX=0x08, + CTL_ROTATIONY=0x10, + CTL_ROTATIONZ=0x20, + CTL_ROTATION=0x38, + CTL_ALL=0x3F, + }; + + // use a combination of CTL_.. in control_output to specify which + CopyPose(unsigned int control_output=CTL_ALL, unsigned int dynamic_output=CTL_NONE, double armlength=1.0, double accuracy=1e-6, unsigned int maximum_iterations=100); + virtual ~CopyPose(); + + virtual bool setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep); + virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues); + +private: + struct ConstraintSingleValue m_posData[3]; // index = controlled output in X,Y,Z order + struct ConstraintSingleValue m_rotData[3]; + struct ConstraintValues m_values[2]; // index = group of controlled output, in position, rotation order + Cache* m_cache; + int m_poseCCh; + CacheTS m_poseCTs; + unsigned int m_poseCacheSize; + unsigned int m_outputDynamic; // combination of CTL_... determine which variables are dynamically controlled by the application + unsigned int m_outputControl; // combination of CTL_... determine which output are constrained + unsigned int m_nvalues; // number of elements used in m_values[] + double m_maxerror; + + struct ControlState { + int firsty; // first y index + int ny; // number of y in output + double alpha; + double K; + double tolerance; + struct ControlValue { + double yddot; + double yd; + double nextyd; + double nextyddot; + } output[3]; // inded numbex = same as m_rotData + } m_rot, m_pos; + + void pushPose(CacheTS timestamp); + bool popPose(CacheTS timestamp); + int nBitsOn(unsigned int v) + { int n=0; while(v) { if (v&1) n++; v>>=1; } return n; } + double* restoreValues(double* item, ConstraintValues* _values, ControlState* _state, unsigned int mask); + double* pushValues(double* item, ControlState* _state, unsigned int mask); + void updateState(ConstraintValues* _values, ControlState* _state, unsigned int mask, double timestep); + void updateValues(Vector& vel, ConstraintValues* _values, ControlState* _state, unsigned int mask); + void updateOutput(Vector& vel, ControlState* _state, unsigned int mask); + void interpolateOutput(ControlState* _state, unsigned int mask, const Timestamp& timestamp); + +}; +} +#endif /* COPYROTATION_H_ */ diff --git a/intern/itasc/Distance.cpp b/intern/itasc/Distance.cpp new file mode 100644 index 00000000000..bf19a978888 --- /dev/null +++ b/intern/itasc/Distance.cpp @@ -0,0 +1,321 @@ +/* $Id$ + * Distance.cpp + * + * Created on: Jan 30, 2009 + * Author: rsmits + */ + +#include "Distance.hpp" +#include "kdl/kinfam_io.hpp" +#include <math.h> +#include <string.h> + +namespace iTaSC +{ +// a distance constraint is characterized by 5 values: alpha, tolerance, K, yd, yddot +static const unsigned int distanceCacheSize = sizeof(double)*5 + sizeof(e_scalar)*6; + +Distance::Distance(double armlength, double accuracy, unsigned int maximum_iterations): + ConstraintSet(1,accuracy,maximum_iterations), + m_chiKdl(6),m_jac(6),m_cache(NULL), + m_distCCh(-1),m_distCTs(0) +{ + m_chain.addSegment(Segment(Joint(Joint::RotZ))); + m_chain.addSegment(Segment(Joint(Joint::RotX))); + m_chain.addSegment(Segment(Joint(Joint::TransY))); + m_chain.addSegment(Segment(Joint(Joint::RotZ))); + m_chain.addSegment(Segment(Joint(Joint::RotY))); + m_chain.addSegment(Segment(Joint(Joint::RotX))); + + m_fksolver = new KDL::ChainFkSolverPos_recursive(m_chain); + m_jacsolver = new KDL::ChainJntToJacSolver(m_chain); + m_Cf(0,2)=1.0; + m_alpha = 1.0; + m_tolerance = 0.05; + m_maxerror = armlength/2.0; + m_K = 20.0; + m_Wy(0) = m_alpha/*/(m_tolerance*m_K)*/; + m_yddot = m_nextyddot = 0.0; + m_yd = m_nextyd = KDL::epsilon; + memset(&m_data, 0, sizeof(m_data)); + // initialize the data with normally fixed values + m_data.id = ID_DISTANCE; + m_values.id = ID_DISTANCE; + m_values.number = 1; + m_values.alpha = m_alpha; + m_values.feedback = m_K; + m_values.tolerance = m_tolerance; + m_values.values = &m_data; +} + +Distance::~Distance() +{ + delete m_fksolver; + delete m_jacsolver; +} + +bool Distance::computeChi(Frame& pose) +{ + double dist, alpha, beta, gamma; + dist = pose.p.Norm(); + Rotation basis; + if (dist < KDL::epsilon) { + // distance is almost 0, no need for initial rotation + m_chi(0) = 0.0; + m_chi(1) = 0.0; + } else { + // find the XZ angles that bring the Y axis to point to init_pose.p + Vector axis(pose.p/dist); + beta = 0.0; + if (fabs(axis(2)) > 1-KDL::epsilon) { + // direction is aligned on Z axis, just rotation on X + alpha = 0.0; + gamma = KDL::sign(axis(2))*KDL::PI/2; + } else { + alpha = -KDL::atan2(axis(0), axis(1)); + gamma = KDL::atan2(axis(2), KDL::sqrt(KDL::sqr(axis(0))+KDL::sqr(axis(1)))); + } + // rotation after first 2 joints + basis = Rotation::EulerZYX(alpha, beta, gamma); + m_chi(0) = alpha; + m_chi(1) = gamma; + } + m_chi(2) = dist; + basis = basis.Inverse()*pose.M; + basis.GetEulerZYX(alpha, beta, gamma); + // alpha = rotation on Z + // beta = rotation on Y + // gamma = rotation on X in that order + // it corresponds to the joint order, so just assign + m_chi(3) = alpha; + m_chi(4) = beta; + m_chi(5) = gamma; + return true; +} + +bool Distance::initialise(Frame& init_pose) +{ + // we will initialize m_chi to values that match the pose + m_externalPose=init_pose; + computeChi(m_externalPose); + // get current Jf and update internal pose + updateJacobian(); + return true; +} + +bool Distance::closeLoop() +{ + if (!Equal(m_internalPose.Inverse()*m_externalPose,F_identity,m_threshold)){ + computeChi(m_externalPose); + updateJacobian(); + } + return true; +} + +void Distance::initCache(Cache *_cache) +{ + m_cache = _cache; + m_distCCh = -1; + if (m_cache) { + // create one channel for the coordinates + m_distCCh = m_cache->addChannel(this, "Xf", distanceCacheSize); + // save initial constraint in cache position 0 + pushDist(0); + } +} + +void Distance::pushDist(CacheTS timestamp) +{ + if (m_distCCh >= 0) { + double *item = (double*)m_cache->addCacheItem(this, m_distCCh, timestamp, NULL, distanceCacheSize); + if (item) { + *item++ = m_K; + *item++ = m_tolerance; + *item++ = m_yd; + *item++ = m_yddot; + *item++ = m_alpha; + memcpy(item, &m_chi[0], 6*sizeof(e_scalar)); + } + m_distCTs = timestamp; + } +} + +bool Distance::popDist(CacheTS timestamp) +{ + if (m_distCCh >= 0) { + double *item = (double*)m_cache->getPreviousCacheItem(this, m_distCCh, ×tamp); + if (item && timestamp != m_distCTs) { + m_values.feedback = m_K = *item++; + m_values.tolerance = m_tolerance = *item++; + m_yd = *item++; + m_yddot = *item++; + m_values.alpha = m_alpha = *item++; + memcpy(&m_chi[0], item, 6*sizeof(e_scalar)); + m_distCTs = timestamp; + m_Wy(0) = m_alpha/*/(m_tolerance*m_K)*/; + updateJacobian(); + } + return (item) ? true : false; + } + return true; +} + +void Distance::pushCache(const Timestamp& timestamp) +{ + if (!timestamp.substep && timestamp.cache) + pushDist(timestamp.cacheTimestamp); +} + +void Distance::updateKinematics(const Timestamp& timestamp) +{ + if (timestamp.interpolate) { + //the internal pose and Jf is already up to date (see model_update) + //update the desired output based on yddot + if (timestamp.substep) { + m_yd += m_yddot*timestamp.realTimestep; + if (m_yd < KDL::epsilon) + m_yd = KDL::epsilon; + } else { + m_yd = m_nextyd; + m_yddot = m_nextyddot; + } + } + pushCache(timestamp); +} + +void Distance::updateJacobian() +{ + for(unsigned int i=0;i<6;i++) + m_chiKdl(i)=m_chi(i); + + m_fksolver->JntToCart(m_chiKdl,m_internalPose); + m_jacsolver->JntToJac(m_chiKdl,m_jac); + changeRefPoint(m_jac,-m_internalPose.p,m_jac); + for(unsigned int i=0;i<6;i++) + for(unsigned int j=0;j<6;j++) + m_Jf(i,j)=m_jac(i,j); +} + +bool Distance::setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep) +{ + int action = 0; + int i; + ConstraintSingleValue* _data; + + while (_nvalues > 0) { + if (_values->id == ID_DISTANCE) { + if ((_values->action & ACT_ALPHA) && _values->alpha >= 0.0) { + m_alpha = _values->alpha; + action |= ACT_ALPHA; + } + if ((_values->action & ACT_TOLERANCE) && _values->tolerance > KDL::epsilon) { + m_tolerance = _values->tolerance; + action |= ACT_TOLERANCE; + } + if ((_values->action & ACT_FEEDBACK) && _values->feedback > KDL::epsilon) { + m_K = _values->feedback; + action |= ACT_FEEDBACK; + } + for (_data = _values->values, i=0; i<_values->number; i++, _data++) { + if (_data->id == ID_DISTANCE) { + switch (_data->action & (ACT_VALUE|ACT_VELOCITY)) { + case 0: + // no indication, keep current values + break; + case ACT_VELOCITY: + // only the velocity is given estimate the new value by integration + _data->yd = m_yd+_data->yddot*timestep; + // walkthrough for negative value correction + case ACT_VALUE: + // only the value is given, estimate the velocity from previous value + if (_data->yd < KDL::epsilon) + _data->yd = KDL::epsilon; + m_nextyd = _data->yd; + // if the user sets the value, we assume future velocity is zero + // (until the user changes the value again) + m_nextyddot = (_data->action & ACT_VALUE) ? 0.0 : _data->yddot; + if (timestep>0.0) { + m_yddot = (_data->yd-m_yd)/timestep; + } else { + // allow the user to change target instantenously when this function + // if called from setControlParameter with timestep = 0 + m_yddot = m_nextyddot; + m_yd = m_nextyd; + } + break; + case (ACT_VALUE|ACT_VELOCITY): + // the user should not set the value and velocity at the same time. + // In this case, we will assume that he want to set the future value + // and we compute the current value to match the velocity + if (_data->yd < KDL::epsilon) + _data->yd = KDL::epsilon; + m_yd = _data->yd - _data->yddot*timestep; + if (m_yd < KDL::epsilon) + m_yd = KDL::epsilon; + m_nextyd = _data->yd; + m_nextyddot = _data->yddot; + if (timestep>0.0) { + m_yddot = (_data->yd-m_yd)/timestep; + } else { + m_yd = m_nextyd; + m_yddot = m_nextyddot; + } + break; + } + } + } + } + _nvalues--; + _values++; + } + if (action & (ACT_TOLERANCE|ACT_FEEDBACK|ACT_ALPHA)) { + // recompute the weight + m_Wy(0) = m_alpha/*/(m_tolerance*m_K)*/; + } + return true; +} + +const ConstraintValues* Distance::getControlParameters(unsigned int* _nvalues) +{ + *(double*)&m_data.y = m_chi(2); + *(double*)&m_data.ydot = m_ydot(0); + m_data.yd = m_yd; + m_data.yddot = m_yddot; + m_data.action = 0; + m_values.action = 0; + if (_nvalues) + *_nvalues=1; + return &m_values; +} + +void Distance::updateControlOutput(const Timestamp& timestamp) +{ + bool cacheAvail = true; + if (!timestamp.substep) { + if (!timestamp.reiterate) + cacheAvail = popDist(timestamp.cacheTimestamp); + } + if (m_constraintCallback && (m_substep || (!timestamp.reiterate && !timestamp.substep))) { + // initialize first callback the application to get the current values + *(double*)&m_data.y = m_chi(2); + *(double*)&m_data.ydot = m_ydot(0); + m_data.yd = m_yd; + m_data.yddot = m_yddot; + m_data.action = 0; + m_values.action = 0; + if ((*m_constraintCallback)(timestamp, &m_values, 1, m_constraintParam)) { + setControlParameters(&m_values, 1, timestamp.realTimestep); + } + } + if (!cacheAvail || !timestamp.interpolate) { + // first position in cache: set the desired output immediately as we cannot interpolate + m_yd = m_nextyd; + m_yddot = m_nextyddot; + } + double error = m_yd-m_chi(2); + if (KDL::Norm(error) > m_maxerror) + error = KDL::sign(error)*m_maxerror; + m_ydot(0)=m_yddot+m_K*error; +} + +} diff --git a/intern/itasc/Distance.hpp b/intern/itasc/Distance.hpp new file mode 100644 index 00000000000..1366693743e --- /dev/null +++ b/intern/itasc/Distance.hpp @@ -0,0 +1,62 @@ +/* $Id: Distance.hpp 19905 2009-04-23 13:29:54Z ben2610 $ + * Distance.hpp + * + * Created on: Jan 30, 2009 + * Author: rsmits + */ + +#ifndef DISTANCE_HPP_ +#define DISTANCE_HPP_ + +#include "ConstraintSet.hpp" +#include "kdl/chain.hpp" +#include "kdl/chainfksolverpos_recursive.hpp" +#include "kdl/chainjnttojacsolver.hpp" + +namespace iTaSC +{ + +class Distance: public iTaSC::ConstraintSet +{ +protected: + virtual void updateKinematics(const Timestamp& timestamp); + virtual void pushCache(const Timestamp& timestamp); + virtual void updateJacobian(); + virtual bool initialise(Frame& init_pose); + virtual void initCache(Cache *_cache); + virtual void updateControlOutput(const Timestamp& timestamp); + virtual bool closeLoop(); + +public: + enum ID { + ID_DISTANCE=1, + }; + Distance(double armlength=1.0, double accuracy=1e-6, unsigned int maximum_iterations=100); + virtual ~Distance(); + + virtual bool setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep); + virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues); + +private: + bool computeChi(Frame& pose); + KDL::Chain m_chain; + KDL::ChainFkSolverPos_recursive* m_fksolver; + KDL::ChainJntToJacSolver* m_jacsolver; + KDL::JntArray m_chiKdl; + KDL::Jacobian m_jac; + struct ConstraintSingleValue m_data; + struct ConstraintValues m_values; + Cache* m_cache; + int m_distCCh; + CacheTS m_distCTs; + double m_maxerror; + + void pushDist(CacheTS timestamp); + bool popDist(CacheTS timestamp); + + double m_alpha,m_yddot,m_yd,m_nextyd,m_nextyddot,m_K,m_tolerance; +}; + +} + +#endif /* DISTANCE_HPP_ */ diff --git a/intern/itasc/FixedObject.cpp b/intern/itasc/FixedObject.cpp new file mode 100644 index 00000000000..fad77d4825e --- /dev/null +++ b/intern/itasc/FixedObject.cpp @@ -0,0 +1,70 @@ +/* $Id$ + * FixedObject.cpp + * + * Created on: Feb 10, 2009 + * Author: benoitbolsee + */ + +#include "FixedObject.hpp" + +namespace iTaSC{ + + +FixedObject::FixedObject():UncontrolledObject(), + m_finalized(false), m_nframe(0) +{ +} + +FixedObject::~FixedObject() +{ + m_frameArray.clear(); +} + +int FixedObject::addFrame(const std::string& name, const Frame& frame) +{ + if (m_finalized) + return -1; + FrameList::iterator it; + unsigned int i; + for (i=0, it=m_frameArray.begin(); i<m_nframe; i++, it++) { + if (it->first == name) { + // this frame will replace the old frame + it->second = frame; + return i; + } + } + m_frameArray.push_back(FrameList::value_type(name,frame)); + return m_nframe++; +} + +int FixedObject::addEndEffector(const std::string& name) +{ + // verify that this frame name exist + FrameList::iterator it; + unsigned int i; + for (i=0, it=m_frameArray.begin(); i<m_nframe; i++, it++) { + if (it->first == name) { + return i; + } + } + return -1; +} + +void FixedObject::finalize() +{ + if (m_finalized) + return; + initialize(0, m_nframe); + m_finalized = true; +} + +const Frame& FixedObject::getPose(const unsigned int frameIndex) +{ + if (frameIndex < m_nframe) { + return m_frameArray[frameIndex].second; + } else { + return F_identity; + } +} + +} diff --git a/intern/itasc/FixedObject.hpp b/intern/itasc/FixedObject.hpp new file mode 100644 index 00000000000..01ab3355259 --- /dev/null +++ b/intern/itasc/FixedObject.hpp @@ -0,0 +1,45 @@ +/* $Id: FixedObject.hpp 19905 2009-04-23 13:29:54Z ben2610 $ + * FixedObject.h + * + * Created on: Feb 10, 2009 + * Author: benoitbolsee + */ + +#ifndef FIXEDOBJECT_HPP_ +#define FIXEDOBJECT_HPP_ + +#include "UncontrolledObject.hpp" +#include <vector> + + +namespace iTaSC{ + +class FixedObject: public UncontrolledObject { +public: + FixedObject(); + virtual ~FixedObject(); + + int addFrame(const std::string& name, const Frame& frame); + + virtual void updateCoordinates(const Timestamp& timestamp) {}; + virtual int addEndEffector(const std::string& name); + virtual void finalize(); + virtual const Frame& getPose(const unsigned int frameIndex); + virtual void updateKinematics(const Timestamp& timestamp) {}; + virtual void pushCache(const Timestamp& timestamp) {}; + virtual void initCache(Cache *_cache) {}; + +protected: + virtual void updateJacobian() {} +private: + typedef std::vector<std::pair<std::string, Frame> > FrameList; + + bool m_finalized; + unsigned int m_nframe; + FrameList m_frameArray; + +}; + +} + +#endif /* FIXEDOBJECT_H_ */ diff --git a/intern/itasc/Makefile b/intern/itasc/Makefile new file mode 100644 index 00000000000..2be46a017df --- /dev/null +++ b/intern/itasc/Makefile @@ -0,0 +1,53 @@ +# +# $Id$ +# +# ***** BEGIN GPL LICENSE BLOCK ***** +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software Foundation, +# Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +# +# The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. +# All rights reserved. +# +# The Original Code is: all of this file. +# +# Contributor(s): Hans Lambermont +# +# ***** END GPL LICENSE BLOCK ***** +# iksolver main makefile. +# + +include nan_definitions.mk + +LIBNAME = itasc +SOURCEDIR = intern/$(LIBNAME) +DIR = $(OCGDIR)/$(SOURCEDIR) +DIRS = kdl +include nan_subdirs.mk +include nan_compile.mk + +CPPFLAGS += -I. +CPPFLAGS += -I../../extern/Eigen2 + +install: $(ALL_OR_DEBUG) + @[ -d $(NAN_ITASC) ] || mkdir $(NAN_ITASC) + @[ -d $(NAN_ITASC)/lib/$(DEBUG_DIR) ] || mkdir $(NAN_ITASC)/lib/$(DEBUG_DIR) + @../tools/cpifdiff.sh $(DIR)/$(DEBUG_DIR)libitasc.a $(DIR)/$(DEBUG_DIR)libitasc_kdl.a $(DIR)/$(DEBUG_DIR)libitasc_kdl_util.a $(NAN_ITASC)/lib/$(DEBUG_DIR) +ifeq ($(OS),darwin) + ranlib $(NAN_ITASC)/lib/$(DEBUG_DIR)libitasc.a + ranlib $(NAN_ITASC)/lib/$(DEBUG_DIR)libitasc_kdl.a + ranlib $(NAN_ITASC)/lib/$(DEBUG_DIR)libitasc_kdl_util.a +endif +############################## +include nan_subdirs.mk diff --git a/intern/itasc/MovingFrame.cpp b/intern/itasc/MovingFrame.cpp new file mode 100644 index 00000000000..e923b1fab27 --- /dev/null +++ b/intern/itasc/MovingFrame.cpp @@ -0,0 +1,156 @@ +/* $Id$ + * MovingFrame.cpp + * + * Created on: Feb 10, 2009 + * Author: benoitbolsee + */ + +#include "MovingFrame.hpp" +#include <string.h> +namespace iTaSC{ + +static const unsigned int frameCacheSize = (sizeof(((Frame*)0)->p.data)+sizeof(((Frame*)0)->M.data))/sizeof(double); + +MovingFrame::MovingFrame(const Frame& frame):UncontrolledObject(), + m_function(NULL), m_param(NULL), m_velocity(), m_poseCCh(-1), m_poseCTs(0) +{ + m_internalPose = m_nextPose = frame; + initialize(6, 1); + e_matrix& Ju = m_JuArray[0]; + Ju = e_identity_matrix(6,6); +} + +MovingFrame::~MovingFrame() +{ +} + +void MovingFrame::finalize() +{ + updateJacobian(); +} + +void MovingFrame::initCache(Cache *_cache) +{ + m_cache = _cache; + m_poseCCh = -1; + if (m_cache) { + m_poseCCh = m_cache->addChannel(this,"pose",frameCacheSize*sizeof(double)); + // don't store the initial pose, it's causing unnecessary large velocity on the first step + //pushInternalFrame(0); + } +} + +void MovingFrame::pushInternalFrame(CacheTS timestamp) +{ + if (m_poseCCh >= 0) { + double buf[frameCacheSize]; + memcpy(buf, m_internalPose.p.data, sizeof(m_internalPose.p.data)); + memcpy(&buf[sizeof(m_internalPose.p.data)/sizeof(double)], m_internalPose.M.data, sizeof(m_internalPose.M.data)); + + m_cache->addCacheVectorIfDifferent(this, m_poseCCh, timestamp, buf, frameCacheSize, KDL::epsilon); + m_poseCTs = timestamp; + } +} + +// load pose just preceeding timestamp +// return false if no cache position was found +bool MovingFrame::popInternalFrame(CacheTS timestamp) +{ + if (m_poseCCh >= 0) { + char *item; + item = (char *)m_cache->getPreviousCacheItem(this, m_poseCCh, ×tamp); + if (item && m_poseCTs != timestamp) { + memcpy(m_internalPose.p.data, item, sizeof(m_internalPose.p.data)); + item += sizeof(m_internalPose.p.data); + memcpy(m_internalPose.M.data, item, sizeof(m_internalPose.M.data)); + m_poseCTs = timestamp; + // changing the starting pose, recompute the jacobian + updateJacobian(); + } + return (item) ? true : false; + } + // in case of no cache, there is always a previous position + return true; +} + +bool MovingFrame::setFrame(const Frame& frame) +{ + m_internalPose = m_nextPose = frame; + return true; +} + +bool MovingFrame::setCallback(MovingFrameCallback _function, void* _param) +{ + m_function = _function; + m_param = _param; + return true; +} + +void MovingFrame::updateCoordinates(const Timestamp& timestamp) +{ + // don't compute the velocity during substepping, it is assumed constant. + if (!timestamp.substep) { + bool cacheAvail = true; + if (!timestamp.reiterate) { + cacheAvail = popInternalFrame(timestamp.cacheTimestamp); + if (m_function) + (*m_function)(timestamp, m_internalPose, m_nextPose, m_param); + } + // only compute velocity if we have a previous pose + if (cacheAvail && timestamp.interpolate) { + unsigned int iXu; + m_velocity = diff(m_internalPose, m_nextPose, timestamp.realTimestep); + for (iXu=0; iXu<6; iXu++) + m_xudot(iXu) = m_velocity(iXu); + } else if (!timestamp.reiterate) { + // new position is forced, no velocity as we cannot interpolate + m_internalPose = m_nextPose; + m_velocity = Twist::Zero(); + m_xudot = e_zero_vector(6); + // recompute the jacobian + updateJacobian(); + } + } +} + +void MovingFrame::pushCache(const Timestamp& timestamp) +{ + if (!timestamp.substep && timestamp.cache) + pushInternalFrame(timestamp.cacheTimestamp); +} + +void MovingFrame::updateKinematics(const Timestamp& timestamp) +{ + if (timestamp.interpolate) { + if (timestamp.substep) { + // during substepping, update the internal pose from velocity information + Twist localvel = m_internalPose.M.Inverse(m_velocity); + m_internalPose.Integrate(localvel, 1.0/timestamp.realTimestep); + } else { + m_internalPose = m_nextPose; + } + // m_internalPose is updated, recompute the jacobian + updateJacobian(); + } + pushCache(timestamp); +} + +void MovingFrame::updateJacobian() +{ + Twist m_jac; + e_matrix& Ju = m_JuArray[0]; + + //Jacobian is always identity at position on the object, + //we ust change the reference to the world. + //instead of going through complicated jacobian operation, implemented direct formula + Ju(1,3) = m_internalPose.p.z(); + Ju(2,3) = -m_internalPose.p.y(); + Ju(0,4) = -m_internalPose.p.z(); + Ju(2,4) = m_internalPose.p.x(); + Ju(0,5) = m_internalPose.p.y(); + Ju(1,5) = -m_internalPose.p.x(); + // remember that this object has moved + m_updated = true; +} + +} diff --git a/intern/itasc/MovingFrame.hpp b/intern/itasc/MovingFrame.hpp new file mode 100644 index 00000000000..edaa3136a13 --- /dev/null +++ b/intern/itasc/MovingFrame.hpp @@ -0,0 +1,48 @@ +/* $Id: MovingFrame.hpp 19907 2009-04-23 13:41:59Z ben2610 $ + * MovingFrame.h + * + * Created on: Feb 10, 2009 + * Author: benoitbolsee + */ + +#ifndef MOVINGFRAME_HPP_ +#define MOVINGFRAME_HPP_ + +#include "UncontrolledObject.hpp" +#include <vector> + + +namespace iTaSC{ + +typedef bool (*MovingFrameCallback)(const Timestamp& timestamp, const Frame& _current, Frame& _next, void *param); + +class MovingFrame: public UncontrolledObject { +public: + MovingFrame(const Frame& frame=F_identity); + virtual ~MovingFrame(); + + bool setFrame(const Frame& frame); + bool setCallback(MovingFrameCallback _function, void* _param); + + virtual void updateCoordinates(const Timestamp& timestamp); + virtual void updateKinematics(const Timestamp& timestamp); + virtual void pushCache(const Timestamp& timestamp); + virtual void initCache(Cache *_cache); + virtual void finalize(); +protected: + virtual void updateJacobian(); + +private: + void pushInternalFrame(CacheTS timestamp); + bool popInternalFrame(CacheTS timestamp); + MovingFrameCallback m_function; + void* m_param; + Frame m_nextPose; + Twist m_velocity; + int m_poseCCh; // cache channel for pose + unsigned int m_poseCTs; +}; + +} + +#endif /* MOVINGFRAME_H_ */ diff --git a/intern/itasc/Object.hpp b/intern/itasc/Object.hpp new file mode 100644 index 00000000000..5c312cab768 --- /dev/null +++ b/intern/itasc/Object.hpp @@ -0,0 +1,48 @@ +/* $Id: Object.hpp 19907 2009-04-23 13:41:59Z ben2610 $ + * Object.hpp + * + * Created on: Jan 5, 2009 + * Author: rubensmits + */ + +#ifndef OBJECT_HPP_ +#define OBJECT_HPP_ + +#include "Cache.hpp" +#include "kdl/frames.hpp" +#include <string> + +namespace iTaSC{ + +class WorldObject; + +class Object { +public: + enum ObjectType {Controlled, UnControlled}; + static WorldObject world; + +private: + ObjectType m_type; +protected: + Cache *m_cache; + KDL::Frame m_internalPose; + bool m_updated; + virtual void updateJacobian()=0; +public: + Object(ObjectType _type):m_type(_type), m_cache(NULL), m_internalPose(F_identity), m_updated(false) {}; + virtual ~Object(){}; + + virtual int addEndEffector(const std::string& name){return 0;}; + virtual void finalize(){}; + virtual const KDL::Frame& getPose(const unsigned int end_effector=0){return m_internalPose;}; + virtual const ObjectType getType(){return m_type;}; + virtual const unsigned int getNrOfCoordinates(){return 0;}; + virtual void updateKinematics(const Timestamp& timestamp)=0; + virtual void pushCache(const Timestamp& timestamp)=0; + virtual void initCache(Cache *_cache) = 0; + bool updated() {return m_updated;}; + void updated(bool val) {m_updated=val;}; +}; + +} +#endif /* OBJECT_HPP_ */ diff --git a/intern/itasc/SConscript b/intern/itasc/SConscript new file mode 100644 index 00000000000..9e11b6c7119 --- /dev/null +++ b/intern/itasc/SConscript @@ -0,0 +1,11 @@ +#!/usr/bin/python +Import ('env') + +sources = env.Glob('*.cpp') +sources += env.Glob('kdl/*.cpp') +sources += env.Glob('kdl/utilities/*.cpp') + +incs = '. ../../extern/Eigen2' + +env.BlenderLib ('bf_ITASC', sources, Split(incs), [], libtype=['intern','player'], priority=[20,100] ) + diff --git a/intern/itasc/Scene.cpp b/intern/itasc/Scene.cpp new file mode 100644 index 00000000000..8aa423584f1 --- /dev/null +++ b/intern/itasc/Scene.cpp @@ -0,0 +1,543 @@ +/* $Id$ + * Scene.cpp + * + * Created on: Jan 5, 2009 + * Author: rubensmits + */ + +#include "Scene.hpp" +#include "ControlledObject.hpp" +#include "kdl/utilities/svd_eigen_HH.hpp" +#include <cstdio> + +namespace iTaSC { + +class SceneLock : public ControlledObject::JointLockCallback { +private: + Scene* m_scene; + Range m_qrange; + +public: + SceneLock(Scene* scene) : + m_scene(scene), m_qrange(0,0) {} + virtual ~SceneLock() {} + + void setRange(Range& range) + { + m_qrange = range; + } + // lock a joint, no need to update output + virtual void lockJoint(unsigned int q_nr, unsigned int ndof) + { + q_nr += m_qrange.start; + project(m_scene->m_Wq, Range(q_nr, ndof), m_qrange).setZero(); + } + // lock a joint and update output in view of reiteration + virtual void lockJoint(unsigned int q_nr, unsigned int ndof, double* qdot) + { + q_nr += m_qrange.start; + project(m_scene->m_Wq, Range(q_nr, ndof), m_qrange).setZero(); + // update the ouput vector so that the movement of this joint will be + // taken into account and we can put the joint back in its initial position + // which means that the jacobian doesn't need to be changed + for (unsigned int i=0 ;i<ndof ; ++i, ++q_nr) { + m_scene->m_ydot -= m_scene->m_A.col(q_nr)*qdot[i]; + } + } +}; + +Scene::Scene(): + m_A(), m_B(), m_Atemp(), m_Wq(), m_Jf(), m_Jq(), m_Ju(), m_Cf(), m_Cq(), m_Jf_inv(), + m_Vf(),m_Uf(), m_Wy(), m_ydot(), m_qdot(), m_xdot(), m_Sf(),m_tempf(), + m_ncTotal(0),m_nqTotal(0),m_nuTotal(0),m_nsets(0), + m_solver(NULL),m_cache(NULL) +{ + m_minstep = 0.01; + m_maxstep = 0.06; +} + +Scene::~Scene() +{ + ConstraintMap::iterator constraint_it; + while ((constraint_it = constraints.begin()) != constraints.end()) { + delete constraint_it->second; + constraints.erase(constraint_it); + } + ObjectMap::iterator object_it; + while ((object_it = objects.begin()) != objects.end()) { + delete object_it->second; + objects.erase(object_it); + } +} + +bool Scene::setParam(SceneParam paramId, double value) +{ + switch (paramId) { + case MIN_TIMESTEP: + m_minstep = value; + break; + case MAX_TIMESTEP: + m_maxstep = value; + break; + default: + return false; + } + return true; +} + +bool Scene::addObject(const std::string& name, Object* object, UncontrolledObject* base, const std::string& baseFrame) +{ + // finalize the object before adding + object->finalize(); + //Check if Object is controlled or uncontrolled. + if(object->getType()==Object::Controlled){ + int baseFrameIndex = base->addEndEffector(baseFrame); + if (baseFrameIndex < 0) + return false; + std::pair<ObjectMap::iterator, bool> result; + if (base->getNrOfCoordinates() == 0) { + // base is fixed object, no coordinate range + result = objects.insert(ObjectMap::value_type( + name, new Object_struct(object,base,baseFrameIndex, + Range(m_nqTotal,object->getNrOfCoordinates()), + Range(m_ncTotal,((ControlledObject*)object)->getNrOfConstraints()), + Range(0,0)))); + } else { + // base is a moving object, must be in list already + ObjectMap::iterator base_it; + for (base_it=objects.begin(); base_it != objects.end(); base_it++) { + if (base_it->second->object == base) + break; + } + if (base_it == objects.end()) + return false; + result = objects.insert(ObjectMap::value_type( + name, new Object_struct(object,base,baseFrameIndex, + Range(m_nqTotal,object->getNrOfCoordinates()), + Range(m_ncTotal,((ControlledObject*)object)->getNrOfConstraints()), + base_it->second->coordinaterange))); + } + if (!result.second) { + return false; + } + m_nqTotal+=object->getNrOfCoordinates(); + m_ncTotal+=((ControlledObject*)object)->getNrOfConstraints(); + return true; + } + if(object->getType()==Object::UnControlled){ + if ((WorldObject*)base != &Object::world) + return false; + std::pair<ObjectMap::iterator,bool> result = objects.insert(ObjectMap::value_type( + name,new Object_struct(object,base,0, + Range(0,0), + Range(0,0), + Range(m_nuTotal,object->getNrOfCoordinates())))); + if(!result.second) + return false; + m_nuTotal+=object->getNrOfCoordinates(); + return true; + } + return false; +} + +bool Scene::addConstraintSet(const std::string& name,ConstraintSet* task,const std::string& object1,const std::string& object2, const std::string& ee1, const std::string& ee2) +{ + //Check if objects exist: + ObjectMap::iterator object1_it = objects.find(object1); + ObjectMap::iterator object2_it = objects.find(object2); + if(object1_it==objects.end()||object2_it==objects.end()) + return false; + int ee1_index = object1_it->second->object->addEndEffector(ee1); + int ee2_index = object2_it->second->object->addEndEffector(ee2); + if (ee1_index < 0 || ee2_index < 0) + return false; + std::pair<ConstraintMap::iterator,bool> result = + constraints.insert(ConstraintMap::value_type(name,new ConstraintSet_struct( + task,object1_it,ee1_index,object2_it,ee2_index, + Range(m_ncTotal,task->getNrOfConstraints()),Range(6*m_nsets,6)))); + if(!result.second) + return false; + m_ncTotal+=task->getNrOfConstraints(); + m_nsets+=1; + return true; +} + +bool Scene::addSolver(Solver* _solver){ + if(m_solver==NULL){ + m_solver=_solver; + return true; + } + else + return false; +} + +bool Scene::addCache(Cache* _cache){ + if(m_cache==NULL){ + m_cache=_cache; + return true; + } + else + return false; +} + +bool Scene::initialize(){ + + //prepare all matrices: + if (m_ncTotal == 0 || m_nqTotal == 0 || m_nsets == 0) + return false; + + m_A = e_zero_matrix(m_ncTotal,m_nqTotal); + if (m_nuTotal > 0) { + m_B = e_zero_matrix(m_ncTotal,m_nuTotal); + m_xdot = e_zero_vector(m_nuTotal); + m_Ju = e_zero_matrix(6*m_nsets,m_nuTotal); + } + m_Atemp = e_zero_matrix(m_ncTotal,6*m_nsets); + m_ydot = e_zero_vector(m_ncTotal); + m_qdot = e_zero_vector(m_nqTotal); + m_Wq = e_zero_matrix(m_nqTotal,m_nqTotal); + m_Wy = e_zero_vector(m_ncTotal); + m_Jq = e_zero_matrix(6*m_nsets,m_nqTotal); + m_Jf = e_zero_matrix(6*m_nsets,6*m_nsets); + m_Jf_inv = m_Jf; + m_Cf = e_zero_matrix(m_ncTotal,m_Jf.rows()); + m_Cq = e_zero_matrix(m_ncTotal,m_nqTotal); + + bool result=true; + // finalize all objects + for (ObjectMap::iterator it=objects.begin(); it!=objects.end(); ++it) { + Object_struct* os = it->second; + + os->object->initCache(m_cache); + if (os->constraintrange.count > 0) + project(m_Cq,os->constraintrange,os->jointrange) = (((ControlledObject*)(os->object))->getCq()); + } + + m_ytask.resize(m_ncTotal); + bool toggle=true; + int cnt = 0; + //Initialize all ConstraintSets: + for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it){ + //Calculate the external pose: + ConstraintSet_struct* cs = it->second; + Frame external_pose; + getConstraintPose(cs->task, cs, external_pose); + result&=cs->task->initialise(external_pose); + cs->task->initCache(m_cache); + for (int i=0; i<cs->constraintrange.count; i++, cnt++) { + m_ytask[cnt] = toggle; + } + toggle = !toggle; + project(m_Cf,cs->constraintrange,cs->featurerange)=cs->task->getCf(); + } + + if(m_solver!=NULL) + m_solver->init(m_nqTotal,m_ncTotal,m_ytask); + else + return false; + + + return result; +} + +bool Scene::getConstraintPose(ConstraintSet* constraint, void *_param, KDL::Frame& _pose) +{ + // function called from constraint when they need to get the external pose + ConstraintSet_struct* cs = (ConstraintSet_struct*)_param; + // verification, the pointer MUST match + assert (constraint == cs->task); + Object_struct* ob1 = cs->object1->second; + Object_struct* ob2 = cs->object2->second; + //Calculate the external pose: + _pose=(ob1->base->getPose(ob1->baseFrameIndex)*ob1->object->getPose(cs->ee1index)).Inverse()*(ob2->base->getPose(ob2->baseFrameIndex)*ob2->object->getPose(cs->ee2index)); + return true; +} + +bool Scene::update(double timestamp, double timestep, unsigned int numsubstep, bool reiterate, bool cache, bool interpolate) +{ + // we must have valid timestep and timestamp + if (timestamp < KDL::epsilon || timestep < 0.0) + return false; + Timestamp ts; + ts.realTimestamp = timestamp; + // initially we start with the full timestep to allow velocity estimation over the full interval + ts.realTimestep = timestep; + setCacheTimestamp(ts); + ts.substep = 0; + // for reiteration don't load cache + // reiteration=additional iteration with same timestamp if application finds the convergence not good enough + ts.reiterate = (reiterate) ? 1 : 0; + ts.interpolate = (interpolate) ? 1 : 0; + ts.cache = (cache) ? 1 : 0; + ts.update = 1; + ts.numstep = (numsubstep & 0xFF); + bool autosubstep = (numsubstep == 0) ? true : false; + if (numsubstep < 1) + numsubstep = 1; + double timesubstep = timestep/numsubstep; + double timeleft = timestep; + + if (timeleft == 0.0) { + // this special case correspond to a request to cache data + for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it){ + it->second->object->pushCache(ts); + } + //Update the Constraints + for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it){ + it->second->task->pushCache(ts); + } + return true; + } + + double maxqdot; + e_scalar nlcoef; + SceneLock lockCallback(this); + Frame external_pose; + bool locked; + + // initially we keep timestep unchanged so that update function compute the velocity over + while (numsubstep > 0) { + // get objects + for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it) { + Object_struct* os = it->second; + if (os->object->getType()==Object::Controlled) { + ((ControlledObject*)(os->object))->updateControlOutput(ts); + if (os->constraintrange.count > 0) { + project(m_ydot, os->constraintrange) = ((ControlledObject*)(os->object))->getControlOutput(); + project(m_Wy, os->constraintrange) = ((ControlledObject*)(os->object))->getWy(); + // project(m_Cq,os->constraintrange,os->jointrange) = (((ControlledObject*)(os->object))->getCq()); + } + if (os->jointrange.count > 0) { + project(m_Wq,os->jointrange,os->jointrange) = ((ControlledObject*)(os->object))->getWq(); + } + } + if (os->object->getType()==Object::UnControlled && ((UncontrolledObject*)os->object)->getNrOfCoordinates() != 0) { + ((UncontrolledObject*)(os->object))->updateCoordinates(ts); + if (!ts.substep) { + // velocity of uncontrolled object remains constant during substepping + project(m_xdot,os->coordinaterange) = ((UncontrolledObject*)(os->object))->getXudot(); + } + } + } + + //get new Constraints values + for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it) { + ConstraintSet_struct* cs = it->second; + Object_struct* ob1 = cs->object1->second; + Object_struct* ob2 = cs->object2->second; + + if (ob1->base->updated() || ob1->object->updated() || ob2->base->updated() || ob2->object->updated()) { + // the object from which the constraint depends have changed position + // recompute the constraint pose + getConstraintPose(cs->task, cs, external_pose); + cs->task->initialise(external_pose); + } + cs->task->updateControlOutput(ts); + project(m_ydot,cs->constraintrange)=cs->task->getControlOutput(); + if (!ts.substep || cs->task->substep()) { + project(m_Wy,cs->constraintrange)=(cs->task)->getWy(); + //project(m_Cf,cs->constraintrange,cs->featurerange)=cs->task->getCf(); + } + + project(m_Jf,cs->featurerange,cs->featurerange)=cs->task->getJf(); + //std::cout << "Jf = " << Jf << std::endl; + //Transform the reference frame of this jacobian to the world reference frame + Eigen::Block<e_matrix> Jf_part = project(m_Jf,cs->featurerange,cs->featurerange); + changeBase(Jf_part,ob1->base->getPose(ob1->baseFrameIndex)*ob1->object->getPose(cs->ee1index)); + //std::cout << "Jf_w = " << Jf << std::endl; + + //calculate the inverse of Jf + KDL::svd_eigen_HH(project(m_Jf,cs->featurerange,cs->featurerange),m_Uf,m_Sf,m_Vf,m_tempf); + for(unsigned int i=0;i<6;++i) + if(m_Sf(i)<KDL::epsilon) + m_Uf.col(i).setConstant(0.0); + else + m_Uf.col(i)*=(1/m_Sf(i)); + project(m_Jf_inv,cs->featurerange,cs->featurerange)=(m_Vf*m_Uf.transpose()).lazy(); + + //Get the robotjacobian associated with this constraintset + //Each jacobian is expressed in robot base frame => convert to world reference + //and negate second robot because it is taken reversed when closing the loop: + if(ob1->object->getType()==Object::Controlled){ + project(m_Jq,cs->featurerange,ob1->jointrange) = (((ControlledObject*)(ob1->object))->getJq(cs->ee1index)); + //Transform the reference frame of this jacobian to the world reference frame: + Eigen::Block<e_matrix> Jq_part = project(m_Jq,cs->featurerange,ob1->jointrange); + changeBase(Jq_part,ob1->base->getPose(ob1->baseFrameIndex)); + // if the base of this object is moving, get the Ju part + if (ob1->base->getNrOfCoordinates() != 0) { + // Ju is already computed for world reference frame + project(m_Ju,cs->featurerange,ob1->coordinaterange)=ob1->base->getJu(ob1->baseFrameIndex); + } + } else if (ob1->object->getType() == Object::UnControlled && ((UncontrolledObject*)ob1->object)->getNrOfCoordinates() != 0) { + // object1 is uncontrolled moving object + project(m_Ju,cs->featurerange,ob1->coordinaterange)=((UncontrolledObject*)ob1->object)->getJu(cs->ee1index); + } + if(ob2->object->getType()==Object::Controlled){ + //Get the robotjacobian associated with this constraintset + // process a special case where object2 and object1 are equal but using different end effector + if (ob1->object == ob2->object) { + // we must create a temporary matrix + e_matrix JqTemp(((ControlledObject*)(ob2->object))->getJq(cs->ee2index)); + //Transform the reference frame of this jacobian to the world reference frame: + changeBase(JqTemp,ob2->base->getPose(ob2->baseFrameIndex)); + // substract in place + project(m_Jq,cs->featurerange,ob2->jointrange) -= JqTemp; + } else { + project(m_Jq,cs->featurerange,ob2->jointrange) = -(((ControlledObject*)(ob2->object))->getJq(cs->ee2index)); + //Transform the reference frame of this jacobian to the world reference frame: + Eigen::Block<e_matrix> Jq_part = project(m_Jq,cs->featurerange,ob2->jointrange); + changeBase(Jq_part,ob2->base->getPose(ob2->baseFrameIndex)); + } + if (ob2->base->getNrOfCoordinates() != 0) { + // if base is the same as first object or first object base, + // that portion of m_Ju has been set already => substract inplace + if (ob2->base == ob1->base || ob2->base == ob1->object) { + project(m_Ju,cs->featurerange,ob2->coordinaterange) -= ob2->base->getJu(ob2->baseFrameIndex); + } else { + project(m_Ju,cs->featurerange,ob2->coordinaterange) = -ob2->base->getJu(ob2->baseFrameIndex); + } + } + } else if (ob2->object->getType() == Object::UnControlled && ((UncontrolledObject*)ob2->object)->getNrOfCoordinates() != 0) { + if (ob2->object == ob1->base || ob2->object == ob1->object) { + project(m_Ju,cs->featurerange,ob2->coordinaterange) -= ((UncontrolledObject*)ob2->object)->getJu(cs->ee2index); + } else { + project(m_Ju,cs->featurerange,ob2->coordinaterange) = -((UncontrolledObject*)ob2->object)->getJu(cs->ee2index); + } + } + } + + //Calculate A + m_Atemp=(m_Cf*m_Jf_inv).lazy(); + m_A = m_Cq-(m_Atemp*m_Jq).lazy(); + if (m_nuTotal > 0) { + m_B=(m_Atemp*m_Ju).lazy(); + m_ydot += (m_B*m_xdot).lazy(); + } + + //Call the solver with A, Wq, Wy, ydot to solver qdot: + if(!m_solver->solve(m_A,m_Wy,m_ydot,m_Wq,m_qdot,nlcoef)) + // this should never happen + return false; + //send result to the objects + for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it) { + Object_struct* os = it->second; + if(os->object->getType()==Object::Controlled) + ((ControlledObject*)(os->object))->setJointVelocity(project(m_qdot,os->jointrange)); + } + // compute the constraint velocity + for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it){ + ConstraintSet_struct* cs = it->second; + Object_struct* ob1 = cs->object1->second; + Object_struct* ob2 = cs->object2->second; + //Calculate the twist of the world reference frame due to the robots (Jq*qdot+Ju*chiudot): + e_vector6 external_vel = e_zero_vector(6); + if (ob1->jointrange.count > 0) + external_vel += (project(m_Jq,cs->featurerange,ob1->jointrange)*project(m_qdot,ob1->jointrange)).lazy(); + if (ob2->jointrange.count > 0) + external_vel += (project(m_Jq,cs->featurerange,ob2->jointrange)*project(m_qdot,ob2->jointrange)).lazy(); + if (ob1->coordinaterange.count > 0) + external_vel += (project(m_Ju,cs->featurerange,ob1->coordinaterange)*project(m_xdot,ob1->coordinaterange)).lazy(); + if (ob2->coordinaterange.count > 0) + external_vel += (project(m_Ju,cs->featurerange,ob2->coordinaterange)*project(m_xdot,ob2->coordinaterange)).lazy(); + //the twist caused by the constraint must be opposite because of the closed loop + //estimate the velocity of the joints using the inverse jacobian + e_vector6 estimated_chidot = project(m_Jf_inv,cs->featurerange,cs->featurerange)*(-external_vel); + cs->task->setJointVelocity(estimated_chidot); + } + + if (autosubstep) { + // automatic computing of substep based on maximum joint change + // and joint limit gain variation + // We will pass the joint velocity to each object and they will recommend a maximum timestep + timesubstep = timeleft; + // get armature max joint velocity to estimate the maximum duration of integration + maxqdot = m_qdot.cwise().abs().maxCoeff(); + double maxsubstep = nlcoef*m_maxstep; + if (maxsubstep < m_minstep) + maxsubstep = m_minstep; + if (timesubstep > maxsubstep) + timesubstep = maxsubstep; + for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it){ + Object_struct* os = it->second; + if(os->object->getType()==Object::Controlled) + ((ControlledObject*)(os->object))->getMaxTimestep(timesubstep); + } + for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it){ + ConstraintSet_struct* cs = it->second; + cs->task->getMaxTimestep(timesubstep); + } + // use substep that are even dividers of timestep for more regularity + maxsubstep = 2.0*floor(timestep/2.0/timesubstep-0.66666); + timesubstep = (maxsubstep < 0.0) ? timestep : timestep/(2.0+maxsubstep); + if (timesubstep >= timeleft-(m_minstep/2.0)) { + timesubstep = timeleft; + numsubstep = 1; + timeleft = 0.; + } else { + numsubstep = 2; + timeleft -= timesubstep; + } + } + if (numsubstep > 1) { + ts.substep = 1; + } else { + // set substep to false for last iteration so that controlled output + // can be updated in updateKinematics() and model_update)() before next call to Secne::update() + ts.substep = 0; + } + // change timestep so that integration is done correctly + ts.realTimestep = timesubstep; + + do { + ObjectMap::iterator it; + Object_struct* os; + locked = false; + for(it=objects.begin();it!=objects.end();++it){ + os = it->second; + if (os->object->getType()==Object::Controlled) { + lockCallback.setRange(os->jointrange); + if (((ControlledObject*)os->object)->updateJoint(ts, lockCallback)) { + // this means one of the joint was locked and we must rerun + // the solver to update the remaining joints + locked = true; + break; + } + } + } + if (locked) { + // Some rows of m_Wq have been cleared so that the corresponding joint will not move + if(!m_solver->solve(m_A,m_Wy,m_ydot,m_Wq,m_qdot,nlcoef)) + // this should never happen + return false; + + //send result to the objects + for(it=objects.begin();it!=objects.end();++it) { + os = it->second; + if(os->object->getType()==Object::Controlled) + ((ControlledObject*)(os->object))->setJointVelocity(project(m_qdot,os->jointrange)); + } + } + } while (locked); + + //Update the Objects + for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it){ + it->second->object->updateKinematics(ts); + // mark this object not updated since the constraint will be updated anyway + // this flag is only useful to detect external updates + it->second->object->updated(false); + } + //Update the Constraints + for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it){ + ConstraintSet_struct* cs = it->second; + //Calculate the external pose: + getConstraintPose(cs->task, cs, external_pose); + cs->task->modelUpdate(external_pose,ts); + // update the constraint output and cache + cs->task->updateKinematics(ts); + } + numsubstep--; + } + return true; +} + +} diff --git a/intern/itasc/Scene.hpp b/intern/itasc/Scene.hpp new file mode 100644 index 00000000000..a2d63361d95 --- /dev/null +++ b/intern/itasc/Scene.hpp @@ -0,0 +1,104 @@ +/* $Id: Scene.hpp 20622 2009-06-04 12:47:59Z ben2610 $ + * Scene.hpp + * + * Created on: Jan 5, 2009 + * Author: rubensmits + */ + +#ifndef SCENE_HPP_ +#define SCENE_HPP_ + +#include "eigen_types.hpp" + +#include "WorldObject.hpp" +#include "ConstraintSet.hpp" +#include "Solver.hpp" + +#include <map> + +namespace iTaSC { + +class SceneLock; + +class Scene { + friend class SceneLock; +public: + enum SceneParam { + MIN_TIMESTEP = 0, + MAX_TIMESTEP, + + COUNT + }; + + + Scene(); + virtual ~Scene(); + + bool addObject(const std::string& name, Object* object, UncontrolledObject* base=&Object::world, const std::string& baseFrame=""); + bool addConstraintSet(const std::string& name, ConstraintSet* task,const std::string& object1,const std::string& object2,const std::string& ee1="",const std::string& ee2=""); + bool addSolver(Solver* _solver); + bool addCache(Cache* _cache); + bool initialize(); + bool update(double timestamp, double timestep, unsigned int numsubstep=1, bool reiterate=false, bool cache=true, bool interpolate=true); + bool setParam(SceneParam paramId, double value); + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +private: + e_matrix m_A,m_B,m_Atemp,m_Wq,m_Jf,m_Jq,m_Ju,m_Cf,m_Cq,m_Jf_inv; + e_matrix6 m_Vf,m_Uf; + e_vector m_Wy,m_ydot,m_qdot,m_xdot; + e_vector6 m_Sf,m_tempf; + double m_minstep; + double m_maxstep; + unsigned int m_ncTotal,m_nqTotal,m_nuTotal,m_nsets; + std::vector<bool> m_ytask; + + Solver* m_solver; + Cache* m_cache; + + + struct Object_struct{ + Object* object; + UncontrolledObject* base; + unsigned int baseFrameIndex; + Range constraintrange; + Range jointrange; + Range coordinaterange; // Xu range of base when object is controlled + // Xu range of object when object is uncontrolled + + Object_struct(Object* _object,UncontrolledObject* _base,unsigned int _baseFrameIndex,Range nq_range,Range nc_range,Range nu_range): + object(_object),base(_base),baseFrameIndex(_baseFrameIndex),constraintrange(nc_range),jointrange(nq_range),coordinaterange(nu_range) + {}; + }; + typedef std::map<std::string,Object_struct*> ObjectMap; + + struct ConstraintSet_struct{ + ConstraintSet* task; + ObjectMap::iterator object1; + ObjectMap::iterator object2; + Range constraintrange; + Range featurerange; + unsigned int ee1index; + unsigned int ee2index; + ConstraintSet_struct(ConstraintSet* _task, + ObjectMap::iterator _object1,unsigned int _ee1index, + ObjectMap::iterator _object2,unsigned int _ee2index, + Range nc_range,Range coord_range): + task(_task), + object1(_object1),object2(_object2), + constraintrange(nc_range),featurerange(coord_range), + ee1index(_ee1index), ee2index(_ee2index) + {}; + }; + typedef std::map<std::string,ConstraintSet_struct*> ConstraintMap; + + ObjectMap objects; + ConstraintMap constraints; + + static bool getConstraintPose(ConstraintSet* constraint, void *_param, KDL::Frame& _pose); +}; + +} + +#endif /* SCENE_HPP_ */ diff --git a/intern/itasc/Solver.hpp b/intern/itasc/Solver.hpp new file mode 100644 index 00000000000..e3aa1e1abc8 --- /dev/null +++ b/intern/itasc/Solver.hpp @@ -0,0 +1,33 @@ +/* $Id: Solver.hpp 20622 2009-06-04 12:47:59Z ben2610 $ + * Solver.hpp + * + * Created on: Jan 8, 2009 + * Author: rubensmits + */ + +#ifndef SOLVER_HPP_ +#define SOLVER_HPP_ + +#include <vector> +#include "eigen_types.hpp" + +namespace iTaSC{ + +class Solver{ +public: + enum SolverParam { + DLS_QMAX = 0, + DLS_LAMBDA_MAX, + DLS_EPSILON + }; + virtual ~Solver(){}; + + // gc = grouping of constraint output , + // size of vector = nc, alternance of true / false to indicate the grouping of output + virtual bool init(unsigned int nq, unsigned int nc, const std::vector<bool>& gc)=0; + virtual bool solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef)=0; + virtual void setParam(SolverParam param, double value)=0; +}; + +} +#endif /* SOLVER_HPP_ */ diff --git a/intern/itasc/UncontrolledObject.cpp b/intern/itasc/UncontrolledObject.cpp new file mode 100644 index 00000000000..4db44aaf7dc --- /dev/null +++ b/intern/itasc/UncontrolledObject.cpp @@ -0,0 +1,43 @@ +/* $Id$ + * UncontrolledObject.cpp + * + * Created on: Jan 5, 2009 + * Author: rubensmits + */ + +#include "UncontrolledObject.hpp" + +namespace iTaSC{ + +UncontrolledObject::UncontrolledObject():Object(UnControlled), + m_nu(0), m_nf(0), m_xudot() +{ +} + +UncontrolledObject::~UncontrolledObject() +{ +} + +void UncontrolledObject::initialize(unsigned int _nu, unsigned int _nf) +{ + assert (_nf >= 1); + m_nu = _nu; + m_nf = _nf; + if (_nu > 0) + m_xudot = e_zero_vector(_nu); + // clear all Jacobian if any + m_JuArray.clear(); + // reserve one more to have an zero matrix handy + if (m_nu > 0) + m_JuArray.resize(m_nf+1, e_zero_matrix(6,m_nu)); +} + +const e_matrix& UncontrolledObject::getJu(unsigned int frameIndex) const +{ + assert (m_nu > 0); + return m_JuArray[(frameIndex>m_nf)?m_nf:frameIndex]; +} + + + +} diff --git a/intern/itasc/UncontrolledObject.hpp b/intern/itasc/UncontrolledObject.hpp new file mode 100644 index 00000000000..3b693a0b2ed --- /dev/null +++ b/intern/itasc/UncontrolledObject.hpp @@ -0,0 +1,37 @@ +/* $Id: UncontrolledObject.hpp 19907 2009-04-23 13:41:59Z ben2610 $ + * UncontrolledObject.h + * + * Created on: Jan 5, 2009 + * Author: rubensmits + */ + +#ifndef UNCONTROLLEDOBJECT_HPP_ +#define UNCONTROLLEDOBJECT_HPP_ + +#include "eigen_types.hpp" + +#include "Object.hpp" +namespace iTaSC{ + +class UncontrolledObject: public Object { +protected: + unsigned int m_nu, m_nf; + e_vector m_xudot; + std::vector<e_matrix> m_JuArray; + +public: + UncontrolledObject(); + virtual ~UncontrolledObject(); + + virtual void initialize(unsigned int _nu, unsigned int _nf); + virtual const e_matrix& getJu(unsigned int frameIndex) const; + virtual const e_vector& getXudot() const {return m_xudot;} + virtual void updateCoordinates(const Timestamp& timestamp)=0; + virtual const unsigned int getNrOfCoordinates(){return m_nu;}; + virtual const unsigned int getNrOfFrames(){return m_nf;}; + +}; + +} + +#endif /* UNCONTROLLEDOBJECT_H_ */ diff --git a/intern/itasc/WDLSSolver.cpp b/intern/itasc/WDLSSolver.cpp new file mode 100644 index 00000000000..1d0efde54c9 --- /dev/null +++ b/intern/itasc/WDLSSolver.cpp @@ -0,0 +1,101 @@ +/* $Id$ + * WDLSSolver.hpp.cpp + * + * Created on: Jan 8, 2009 + * Author: rubensmits + */ + +#include "WDLSSolver.hpp" +#include "kdl/utilities/svd_eigen_HH.hpp" + +namespace iTaSC { + +WDLSSolver::WDLSSolver() : m_lambda(0.5), m_epsilon(0.1) +{ + // maximum joint velocity + m_qmax = 50.0; +} + +WDLSSolver::~WDLSSolver() { +} + +bool WDLSSolver::init(unsigned int nq, unsigned int nc, const std::vector<bool>& gc) +{ + m_ns = std::min(nc,nq); + m_AWq = e_zero_matrix(nc,nq); + m_WyAWq = e_zero_matrix(nc,nq); + m_WyAWqt = e_zero_matrix(nq,nc); + m_S = e_zero_vector(std::max(nc,nq)); + m_Wy_ydot = e_zero_vector(nc); + if (nq > nc) { + m_transpose = true; + m_temp = e_zero_vector(nc); + m_U = e_zero_matrix(nc,nc); + m_V = e_zero_matrix(nq,nc); + m_WqV = e_zero_matrix(nq,nc); + } else { + m_transpose = false; + m_temp = e_zero_vector(nq); + m_U = e_zero_matrix(nc,nq); + m_V = e_zero_matrix(nq,nq); + m_WqV = e_zero_matrix(nq,nq); + } + return true; +} + +bool WDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef) +{ + double alpha, vmax, norm; + // Create the Weighted jacobian + m_AWq = A*Wq; + for (int i=0; i<Wy.size(); i++) + m_WyAWq.row(i) = Wy(i)*m_AWq.row(i); + + // Compute the SVD of the weighted jacobian + int ret; + if (m_transpose) { + m_WyAWqt = m_WyAWq.transpose(); + ret = KDL::svd_eigen_HH(m_WyAWqt,m_V,m_S,m_U,m_temp); + } else { + ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp); + } + if(ret<0) + return false; + + m_WqV = (Wq*m_V).lazy(); + + //Wy*ydot + m_Wy_ydot = Wy.cwise() * ydot; + //S^-1*U'*Wy*ydot + e_scalar maxDeltaS = e_scalar(0.0); + e_scalar prevS = e_scalar(0.0); + e_scalar maxS = e_scalar(1.0); + e_scalar S, lambda; + qdot.setZero(); + for(int i=0;i<m_ns;++i) { + S = m_S(i); + if (S <= KDL::epsilon) + break; + if (i > 0 && (prevS-S) > maxDeltaS) { + maxDeltaS = (prevS-S); + maxS = prevS; + } + lambda = (S < m_epsilon) ? (e_scalar(1.0)-KDL::sqr(S/m_epsilon))*m_lambda*m_lambda : e_scalar(0.0); + alpha = m_U.col(i).dot(m_Wy_ydot)*S/(S*S+lambda); + vmax = m_WqV.col(i).cwise().abs().maxCoeff(); + norm = fabs(alpha*vmax); + if (norm > m_qmax) { + qdot += m_WqV.col(i)*(alpha*m_qmax/norm); + } else { + qdot += m_WqV.col(i)*alpha; + } + prevS = S; + } + if (maxDeltaS == e_scalar(0.0)) + nlcoef = e_scalar(KDL::epsilon); + else + nlcoef = (maxS-maxDeltaS)/maxS; + return true; +} + +} diff --git a/intern/itasc/WDLSSolver.hpp b/intern/itasc/WDLSSolver.hpp new file mode 100644 index 00000000000..b56ad1ab2b8 --- /dev/null +++ b/intern/itasc/WDLSSolver.hpp @@ -0,0 +1,48 @@ +/* $Id: WDLSSolver.hpp 20622 2009-06-04 12:47:59Z ben2610 $ + * WDLSSolver.hpp + * + * Created on: Jan 8, 2009 + * Author: rubensmits + */ + +#ifndef WDLSSOLVER_HPP_ +#define WDLSSOLVER_HPP_ + +#include "Solver.hpp" + +namespace iTaSC { + +class WDLSSolver: public iTaSC::Solver { +private: + e_matrix m_AWq,m_WyAWq,m_WyAWqt,m_U,m_V,m_WqV; + e_vector m_S,m_temp,m_Wy_ydot; + double m_lambda; + double m_epsilon; + double m_qmax; + int m_ns; + bool m_transpose; +public: + WDLSSolver(); + virtual ~WDLSSolver(); + + virtual bool init(unsigned int nq, unsigned int nc, const std::vector<bool>& gc); + virtual bool solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef); + virtual void setParam(SolverParam param, double value) + { + switch (param) { + case DLS_QMAX: + m_qmax = value; + break; + case DLS_LAMBDA_MAX: + m_lambda = value; + break; + case DLS_EPSILON: + m_epsilon = value; + break; + } + } +}; + +} + +#endif /* WDLSSOLVER_HPP_ */ diff --git a/intern/itasc/WSDLSSolver.cpp b/intern/itasc/WSDLSSolver.cpp new file mode 100644 index 00000000000..9f7ebed960a --- /dev/null +++ b/intern/itasc/WSDLSSolver.cpp @@ -0,0 +1,138 @@ +/* $Id$ + * WDLSSolver.hpp.cpp + * + * Created on: Jan 8, 2009 + * Author: rubensmits + */ + +#include "WSDLSSolver.hpp" +#include "kdl/utilities/svd_eigen_HH.hpp" +#include <cstdio> + +namespace iTaSC { + +WSDLSSolver::WSDLSSolver() : + m_ns(0), m_nc(0), m_nq(0) + +{ + // default maximum speed: 50 rad/s + m_qmax = 50.0; +} + +WSDLSSolver::~WSDLSSolver() { +} + +bool WSDLSSolver::init(unsigned int _nq, unsigned int _nc, const std::vector<bool>& gc) +{ + if (_nc == 0 || _nq == 0 || gc.size() != _nc) + return false; + m_nc = _nc; + m_nq = _nq; + m_ns = std::min(m_nc,m_nq); + m_AWq = e_zero_matrix(m_nc,m_nq); + m_WyAWq = e_zero_matrix(m_nc,m_nq); + m_WyAWqt = e_zero_matrix(m_nq,m_nc); + m_S = e_zero_vector(std::max(m_nc,m_nq)); + m_Wy_ydot = e_zero_vector(m_nc); + m_ytask = gc; + if (m_nq > m_nc) { + m_transpose = true; + m_temp = e_zero_vector(m_nc); + m_U = e_zero_matrix(m_nc,m_nc); + m_V = e_zero_matrix(m_nq,m_nc); + m_WqV = e_zero_matrix(m_nq,m_nc); + } else { + m_transpose = false; + m_temp = e_zero_vector(m_nq); + m_U = e_zero_matrix(m_nc,m_nq); + m_V = e_zero_matrix(m_nq,m_nq); + m_WqV = e_zero_matrix(m_nq,m_nq); + } + return true; +} + +bool WSDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef) +{ + unsigned int i, j, l; + e_scalar N, M; + + // Create the Weighted jacobian + m_AWq = (A*Wq).lazy(); + for (i=0; i<m_nc; i++) + m_WyAWq.row(i) = Wy(i)*m_AWq.row(i); + + // Compute the SVD of the weighted jacobian + int ret; + if (m_transpose) { + m_WyAWqt = m_WyAWq.transpose(); + ret = KDL::svd_eigen_HH(m_WyAWqt,m_V,m_S,m_U,m_temp); + } else { + ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp); + } + if(ret<0) + return false; + + m_Wy_ydot = Wy.cwise() * ydot; + m_WqV = (Wq*m_V).lazy(); + qdot.setZero(); + e_scalar maxDeltaS = e_scalar(0.0); + e_scalar prevS = e_scalar(0.0); + e_scalar maxS = e_scalar(1.0); + for(i=0;i<m_ns;++i) { + e_scalar norm, mag, alpha, _qmax, Sinv, vmax, damp; + e_scalar S = m_S(i); + bool prev; + if (S < KDL::epsilon) + break; + Sinv = e_scalar(1.)/S; + if (i > 0) { + if ((prevS-S) > maxDeltaS) { + maxDeltaS = (prevS-S); + maxS = prevS; + } + } + N = M = e_scalar(0.); + for (l=0, prev=m_ytask[0], norm=e_scalar(0.); l<m_nc; l++) { + if (prev == m_ytask[l]) { + norm += m_U(l,i)*m_U(l,i); + } else { + N += std::sqrt(norm); + norm = m_U(l,i)*m_U(l,i); + } + prev = m_ytask[l]; + } + N += std::sqrt(norm); + for (j=0; j<m_nq; j++) { + for (l=0, prev=m_ytask[0], norm=e_scalar(0.), mag=e_scalar(0.); l<m_nc; l++) { + if (prev == m_ytask[l]) { + norm += m_WyAWq(l,j)*m_WyAWq(l,j); + } else { + mag += std::sqrt(norm); + norm = m_WyAWq(l,j)*m_WyAWq(l,j); + } + prev = m_ytask[l]; + } + mag += std::sqrt(norm); + M += fabs(m_V(j,i))*mag; + } + M *= Sinv; + alpha = m_U.col(i).dot(m_Wy_ydot); + _qmax = (N < M) ? m_qmax*N/M : m_qmax; + vmax = m_WqV.col(i).cwise().abs().maxCoeff(); + norm = fabs(Sinv*alpha*vmax); + if (norm > _qmax) { + damp = Sinv*alpha*_qmax/norm; + } else { + damp = Sinv*alpha; + } + qdot += m_WqV.col(i)*damp; + prevS = S; + } + if (maxDeltaS == e_scalar(0.0)) + nlcoef = e_scalar(KDL::epsilon); + else + nlcoef = (maxS-maxDeltaS)/maxS; + return true; +} + +} diff --git a/intern/itasc/WSDLSSolver.hpp b/intern/itasc/WSDLSSolver.hpp new file mode 100644 index 00000000000..0b17f26ef47 --- /dev/null +++ b/intern/itasc/WSDLSSolver.hpp @@ -0,0 +1,43 @@ +/* $Id: WSDLSSolver.hpp 20622 2009-06-04 12:47:59Z ben2610 $ + * WSDLSSolver.hpp + * + * Created on: Mar 26, 2009 + * Author: benoit bolsee + */ + +#ifndef WSDLSSOLVER_HPP_ +#define WSDLSSOLVER_HPP_ + +#include "Solver.hpp" + +namespace iTaSC { + +class WSDLSSolver: public iTaSC::Solver { +private: + e_matrix m_AWq,m_WyAWq,m_WyAWqt,m_U,m_V,m_WqV; + e_vector m_S,m_temp,m_Wy_ydot; + std::vector<bool> m_ytask; + e_scalar m_qmax; + unsigned int m_ns, m_nc, m_nq; + bool m_transpose; +public: + WSDLSSolver(); + virtual ~WSDLSSolver(); + + virtual bool init(unsigned int _nq, unsigned int _nc, const std::vector<bool>& gc); + virtual bool solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef); + virtual void setParam(SolverParam param, double value) + { + switch (param) { + case DLS_QMAX: + m_qmax = value; + break; + default: + break; + } + } +}; + +} + +#endif /* WSDLSSOLVER_HPP_ */ diff --git a/intern/itasc/WorldObject.cpp b/intern/itasc/WorldObject.cpp new file mode 100644 index 00000000000..99cb8773e77 --- /dev/null +++ b/intern/itasc/WorldObject.cpp @@ -0,0 +1,26 @@ +/* $Id$ + * WorldObject.cpp + * + * Created on: Feb 10, 2009 + * Author: benoitbolsee + */ + +#include "WorldObject.hpp" + +namespace iTaSC{ + +/* special singleton to be used as base for uncontrolled object */ +WorldObject Object::world; + +WorldObject::WorldObject():UncontrolledObject() +{ + initialize(0,1); + m_internalPose = Frame::Identity(); +} + +WorldObject::~WorldObject() +{ +} + + +} diff --git a/intern/itasc/WorldObject.hpp b/intern/itasc/WorldObject.hpp new file mode 100644 index 00000000000..b309545a843 --- /dev/null +++ b/intern/itasc/WorldObject.hpp @@ -0,0 +1,30 @@ +/* $Id: WorldObject.hpp 19907 2009-04-23 13:41:59Z ben2610 $ + * WorldObject.h + * + * Created on: Feb 10, 2009 + * Author: benoitbolsee + */ + +#ifndef WORLDOBJECT_HPP_ +#define WORLDOBJECT_HPP_ + +#include "UncontrolledObject.hpp" +namespace iTaSC{ + +class WorldObject: public UncontrolledObject { +public: + WorldObject(); + virtual ~WorldObject(); + + virtual void updateCoordinates(const Timestamp& timestamp) {}; + virtual void updateKinematics(const Timestamp& timestamp) {}; + virtual void pushCache(const Timestamp& timestamp) {}; + virtual void initCache(Cache *_cache) {}; +protected: + virtual void updateJacobian() {} + +}; + +} + +#endif /* WORLDOBJECT_H_ */ diff --git a/intern/itasc/eigen_types.cpp b/intern/itasc/eigen_types.cpp new file mode 100644 index 00000000000..2aa942f38c7 --- /dev/null +++ b/intern/itasc/eigen_types.cpp @@ -0,0 +1,12 @@ +/* $Id$ + * eigen_types.cpp + * + * Created on: March 19, 2009 + * Author: benoit bolsee + */ + +#include "eigen_types.hpp" + +const KDL::Frame iTaSC::F_identity(Rotation::Identity(),Vector::Zero()); + + diff --git a/intern/itasc/eigen_types.hpp b/intern/itasc/eigen_types.hpp new file mode 100644 index 00000000000..fe46f8b6bb3 --- /dev/null +++ b/intern/itasc/eigen_types.hpp @@ -0,0 +1,84 @@ +/* $Id: eigen_types.hpp 19905 2009-04-23 13:29:54Z ben2610 $ + * eigen_types.hpp + * + * Created on: March 6, 2009 + * Author: benoit bolsee + */ + +#ifndef EIGEN_TYPES_HPP_ +#define EIGEN_TYPES_HPP_ + +#include <Eigen/Core> +#include "kdl/frames.hpp" +#include "kdl/tree.hpp" +#include "kdl/chain.hpp" +#include "kdl/jacobian.hpp" +#include "kdl/jntarray.hpp" + + +namespace iTaSC{ + +using KDL::Twist; +using KDL::Frame; +using KDL::Joint; +using KDL::Inertia; +using KDL::SegmentMap; +using KDL::Tree; +using KDL::JntArray; +using KDL::Jacobian; +using KDL::Segment; +using KDL::Rotation; +using KDL::Vector; +using KDL::Vector2; +using KDL::Chain; + +extern const Frame F_identity; + +#define e_scalar double +#define e_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1> +#define e_zero_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>::Zero +#define e_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic> +#define e_matrix6 Eigen::Matrix<e_scalar,6,6> +#define e_identity_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Identity +#define e_scalar_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>::Constant +#define e_zero_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero +#define e_random_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Random +#define e_vector6 Eigen::Matrix<e_scalar,6,1> +#define e_vector3 Eigen::Matrix<e_scalar,3,1> + +class Range { +public: + int start; + int count; + Range(int _start, int _count) { start = _start; count=_count; } + Range(const Range& other) { start=other.start; count=other.count; } +}; + +template<typename MatrixType> inline Eigen::Block<MatrixType> project(MatrixType& m, Range r) +{ + return Eigen::Block<MatrixType>(m,r.start,0,r.count,1); +} + +template<typename MatrixType> inline Eigen::Block<MatrixType> project(MatrixType& m, Range r, Range c) +{ + return Eigen::Block<MatrixType>(m,r.start,c.start,r.count,c.count); +} + +template<typename Derived> inline static int changeBase(Eigen::MatrixBase<Derived>& J, const Frame& T) { + + if (J.rows() != 6) + return -1; + for (int j = 0; j < J.cols(); ++j) { + typename Derived::ColXpr Jj = J.col(j); + Twist arg; + for(unsigned int i=0;i<6;++i) + arg(i)=Jj[i]; + Twist tmp(T*arg); + for(unsigned int i=0;i<6;++i) + Jj[i]=e_scalar(tmp(i)); + } + return 0; +} + +} +#endif /* UBLAS_TYPES_HPP_ */ diff --git a/intern/itasc/kdl/Makefile b/intern/itasc/kdl/Makefile new file mode 100644 index 00000000000..058f93da4e1 --- /dev/null +++ b/intern/itasc/kdl/Makefile @@ -0,0 +1,43 @@ +# +# $Id$ +# +# ***** BEGIN GPL LICENSE BLOCK ***** +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software Foundation, +# Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +# +# The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. +# All rights reserved. +# +# The Original Code is: all of this file. +# +# Contributor(s): Hans Lambermont +# +# ***** END GPL LICENSE BLOCK ***** +# iksolver main makefile. +# + +include nan_definitions.mk + +LIBNAME = itasc_kdl +# Yep, same dir than parent (itasc instead of $(LIBNAME)) +DIR = $(OCGDIR)/intern/itasc +DIRS = utilities +SOURCEDIR = intern/$(LIBNAME)/kdl + +include nan_subdirs.mk +include nan_compile.mk + +CPPFLAGS += -I. +CPPFLAGS += -I../../../extern/Eigen2 diff --git a/intern/itasc/kdl/chain.cpp b/intern/itasc/kdl/chain.cpp new file mode 100644 index 00000000000..638366c96be --- /dev/null +++ b/intern/itasc/kdl/chain.cpp @@ -0,0 +1,75 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "chain.hpp" + +namespace KDL { + using namespace std; + + Chain::Chain(): + segments(0), + nrOfJoints(0), + nrOfSegments(0) + { + } + + Chain::Chain(const Chain& in):nrOfJoints(0), + nrOfSegments(0) + { + for(unsigned int i=0;i<in.getNrOfSegments();i++) + this->addSegment(in.getSegment(i)); + } + + Chain& Chain::operator=(const Chain& arg) + { + nrOfJoints=0; + nrOfSegments=0; + segments.resize(0); + for(unsigned int i=0;i<arg.nrOfSegments;i++) + addSegment(arg.getSegment(i)); + return *this; + + } + + void Chain::addSegment(const Segment& segment) + { + segments.push_back(segment); + nrOfSegments++; + nrOfJoints += segment.getJoint().getNDof(); + } + + void Chain::addChain(const Chain& chain) + { + for(unsigned int i=0;i<chain.getNrOfSegments();i++) + this->addSegment(chain.getSegment(i)); + } + + const Segment& Chain::getSegment(unsigned int nr) const + { + return segments[nr]; + } + + Chain::~Chain() + { + } + +} + diff --git a/intern/itasc/kdl/chain.hpp b/intern/itasc/kdl/chain.hpp new file mode 100644 index 00000000000..0d40690202a --- /dev/null +++ b/intern/itasc/kdl/chain.hpp @@ -0,0 +1,95 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_CHAIN_HPP +#define KDL_CHAIN_HPP + +#include "segment.hpp" +#include <string> + +namespace KDL { + /** + * \brief This class encapsulates a <strong>serial</strong> kinematic + * interconnection structure. It is build out of segments. + * + * @ingroup KinematicFamily + */ + class Chain { + private: + std::vector<Segment> segments; + unsigned int nrOfJoints; + unsigned int nrOfSegments; + public: + /** + * The constructor of a chain, a new chain is always empty. + * + */ + Chain(); + Chain(const Chain& in); + Chain& operator = (const Chain& arg); + + /** + * Adds a new segment to the <strong>end</strong> of the chain. + * + * @param segment The segment to add + */ + void addSegment(const Segment& segment); + /** + * Adds a complete chain to the <strong>end</strong> of the chain + * The added chain is copied. + * + * @param chain The chain to add + */ + void addChain(const Chain& chain); + + /** + * Request the total number of joints in the chain.\n + * <strong> Important:</strong> It is not the + * same as the total number of segments since a segment does not + * need to have a joint. This function is important when + * creating a KDL::JntArray to use with this chain. + * @return total nr of joints + */ + unsigned int getNrOfJoints()const {return nrOfJoints;}; + /** + * Request the total number of segments in the chain. + * @return total number of segments + */ + unsigned int getNrOfSegments()const {return nrOfSegments;}; + + /** + * Request the nr'd segment of the chain. There is no boundary + * checking. + * + * @param nr the nr of the segment starting from 0 + * + * @return a constant reference to the nr'd segment + */ + const Segment& getSegment(unsigned int nr)const; + + virtual ~Chain(); + }; + + + +}//end of namespace KDL + +#endif diff --git a/intern/itasc/kdl/chainfksolver.hpp b/intern/itasc/kdl/chainfksolver.hpp new file mode 100644 index 00000000000..fa6f625ee9d --- /dev/null +++ b/intern/itasc/kdl/chainfksolver.hpp @@ -0,0 +1,107 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_CHAIN_FKSOLVER_HPP +#define KDL_CHAIN_FKSOLVER_HPP + +#include "chain.hpp" +#include "framevel.hpp" +#include "frameacc.hpp" +#include "jntarray.hpp" +#include "jntarrayvel.hpp" +#include "jntarrayacc.hpp" + +namespace KDL { + + /** + * \brief This <strong>abstract</strong> class encapsulates a + * solver for the forward position kinematics for a KDL::Chain. + * + * @ingroup KinematicFamily + */ + + //Forward definition + class ChainFkSolverPos { + public: + /** + * Calculate forward position kinematics for a KDL::Chain, + * from joint coordinates to cartesian pose. + * + * @param q_in input joint coordinates + * @param p_out reference to output cartesian pose + * + * @return if < 0 something went wrong + */ + virtual int JntToCart(const JntArray& q_in, Frame& p_out,int segmentNr=-1)=0; + virtual ~ChainFkSolverPos(){}; + }; + + /** + * \brief This <strong>abstract</strong> class encapsulates a solver + * for the forward velocity kinematics for a KDL::Chain. + * + * @ingroup KinematicFamily + */ + class ChainFkSolverVel { + public: + /** + * Calculate forward position and velocity kinematics, from + * joint coordinates to cartesian coordinates. + * + * @param q_in input joint coordinates (position and velocity) + * @param out output cartesian coordinates (position and velocity) + * + * @return if < 0 something went wrong + */ + virtual int JntToCart(const JntArrayVel& q_in, FrameVel& out,int segmentNr=-1)=0; + + virtual ~ChainFkSolverVel(){}; + }; + + /** + * \brief This <strong>abstract</strong> class encapsulates a solver + * for the forward acceleration kinematics for a KDL::Chain. + * + * @ingroup KinematicFamily + */ + + class ChainFkSolverAcc { + public: + /** + * Calculate forward position, velocity and accelaration + * kinematics, from joint coordinates to cartesian coordinates + * + * @param q_in input joint coordinates (position, velocity and + * acceleration + @param out output cartesian coordinates (position, velocity + * and acceleration + * + * @return if < 0 something went wrong + */ + virtual int JntToCart(const JntArrayAcc& q_in, FrameAcc& out,int segmentNr=-1)=0; + + virtual ~ChainFkSolverAcc()=0; + }; + + +}//end of namespace KDL + +#endif diff --git a/intern/itasc/kdl/chainfksolverpos_recursive.cpp b/intern/itasc/kdl/chainfksolverpos_recursive.cpp new file mode 100644 index 00000000000..46c29c9c6e0 --- /dev/null +++ b/intern/itasc/kdl/chainfksolverpos_recursive.cpp @@ -0,0 +1,61 @@ +// Copyright (C) 2007 Francois Cauwe <francois at cauwe dot org> +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "chainfksolverpos_recursive.hpp" +#include <iostream> + +namespace KDL { + + ChainFkSolverPos_recursive::ChainFkSolverPos_recursive(const Chain& _chain): + chain(_chain) + { + } + + int ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr) + { + unsigned int segNr = (unsigned int)segmentNr; + if(segmentNr<0) + segNr=chain.getNrOfSegments(); + + p_out = Frame::Identity(); + + if(q_in.rows()!=chain.getNrOfJoints()) + return -1; + else if(segNr>chain.getNrOfSegments()) + return -1; + else{ + int j=0; + for(unsigned int i=0;i<segNr;i++){ + p_out = p_out*chain.getSegment(i).pose(((JntArray&)q_in)(j)); + j+=chain.getSegment(i).getJoint().getNDof(); + } + return 0; + } + } + + + ChainFkSolverPos_recursive::~ChainFkSolverPos_recursive() + { + } + + +} diff --git a/intern/itasc/kdl/chainfksolverpos_recursive.hpp b/intern/itasc/kdl/chainfksolverpos_recursive.hpp new file mode 100644 index 00000000000..72cdab0b28b --- /dev/null +++ b/intern/itasc/kdl/chainfksolverpos_recursive.hpp @@ -0,0 +1,50 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDLCHAINFKSOLVERPOS_RECURSIVE_HPP +#define KDLCHAINFKSOLVERPOS_RECURSIVE_HPP + +#include "chainfksolver.hpp" + +namespace KDL { + + /** + * Implementation of a recursive forward position kinematics + * algorithm to calculate the position transformation from joint + * space to Cartesian space of a general kinematic chain (KDL::Chain). + * + * @ingroup KinematicFamily + */ + class ChainFkSolverPos_recursive : public ChainFkSolverPos + { + public: + ChainFkSolverPos_recursive(const Chain& chain); + ~ChainFkSolverPos_recursive(); + + virtual int JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr=-1); + + private: + const Chain chain; + }; + +} + +#endif diff --git a/intern/itasc/kdl/chainjnttojacsolver.cpp b/intern/itasc/kdl/chainjnttojacsolver.cpp new file mode 100644 index 00000000000..4a801c041f3 --- /dev/null +++ b/intern/itasc/kdl/chainjnttojacsolver.cpp @@ -0,0 +1,80 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "chainjnttojacsolver.hpp" + +namespace KDL +{ + ChainJntToJacSolver::ChainJntToJacSolver(const Chain& _chain): + chain(_chain) + { + } + + ChainJntToJacSolver::~ChainJntToJacSolver() + { + } + + int ChainJntToJacSolver::JntToJac(const JntArray& q_in,Jacobian& jac) + { + assert(q_in.rows()==chain.getNrOfJoints()&& + q_in.rows()==jac.columns()); + + + Frame T_local, T_joint; + T_total = Frame::Identity(); + SetToZero(t_local); + + int i=chain.getNrOfSegments()-1; + unsigned int q_nr = chain.getNrOfJoints(); + + //Lets recursively iterate until we are in the root segment + while (i >= 0) { + const Segment& segment = chain.getSegment(i); + int ndof = segment.getJoint().getNDof(); + q_nr -= ndof; + + //get the pose of the joint. + T_joint = segment.getJoint().pose(((JntArray&)q_in)(q_nr)); + // combine with the tip to have the tip pose + T_local = T_joint*segment.getFrameToTip(); + //calculate new T_end: + T_total = T_local * T_total; + + for (int dof=0; dof<ndof; dof++) { + // combine joint rotation with tip position to get a reference frame for the joint + T_joint.p = T_local.p; + // in which the twist can be computed (needed for NDof joint) + t_local = segment.twist(T_joint, 1.0, dof); + //transform the endpoint of the local twist to the global endpoint: + t_local = t_local.RefPoint(T_total.p - T_local.p); + //transform the base of the twist to the endpoint + t_local = T_total.M.Inverse(t_local); + //store the twist in the jacobian: + jac.twists[q_nr+dof] = t_local; + } + i--; + }//endwhile + //Change the base of the complete jacobian from the endpoint to the base + changeBase(jac, T_total.M, jac); + return 0; + } +} + diff --git a/intern/itasc/kdl/chainjnttojacsolver.hpp b/intern/itasc/kdl/chainjnttojacsolver.hpp new file mode 100644 index 00000000000..98f9d06ee0d --- /dev/null +++ b/intern/itasc/kdl/chainjnttojacsolver.hpp @@ -0,0 +1,65 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_CHAINJNTTOJACSOLVER_HPP +#define KDL_CHAINJNTTOJACSOLVER_HPP + +#include "frames.hpp" +#include "jacobian.hpp" +#include "jntarray.hpp" +#include "chain.hpp" + +namespace KDL +{ + /** + * @brief Class to calculate the jacobian of a general + * KDL::Chain, it is used by other solvers. It should not be used + * outside of KDL. + * + * + */ + + class ChainJntToJacSolver + { + public: + ChainJntToJacSolver(const Chain& chain); + ~ChainJntToJacSolver(); + /** + * Calculate the jacobian expressed in the base frame of the + * chain, with reference point at the end effector of the + * *chain. The alghoritm is similar to the one used in + * KDL::ChainFkSolverVel_recursive + * + * @param q_in input joint positions + * @param jac output jacobian + * + * @return always returns 0 + */ + int JntToJac(const JntArray& q_in,Jacobian& jac); + + private: + const Chain chain; + Twist t_local; + Frame T_total; + }; +} +#endif + diff --git a/intern/itasc/kdl/frameacc.cpp b/intern/itasc/kdl/frameacc.cpp new file mode 100644 index 00000000000..9defce0a00e --- /dev/null +++ b/intern/itasc/kdl/frameacc.cpp @@ -0,0 +1,26 @@ +/***************************************************************************** + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id$ + * $Name: $ + ****************************************************************************/ + + +#include "frameacc.hpp" + +namespace KDL { + +#ifndef KDL_INLINE + #include "frameacc.inl" +#endif + +} + diff --git a/intern/itasc/kdl/frameacc.hpp b/intern/itasc/kdl/frameacc.hpp new file mode 100644 index 00000000000..4157237222e --- /dev/null +++ b/intern/itasc/kdl/frameacc.hpp @@ -0,0 +1,259 @@ +/***************************************************************************** + * \file + * This file contains the definition of classes for a + * Rall Algebra of (subset of) the classes defined in frames, + * i.e. classes that contain a set (value,derivative,2nd derivative) + * and define operations on that set + * this classes are usefull for automatic differentiation ( <-> symbolic diff , + * <-> numeric diff). + * Defines VectorAcc, RotationAcc, FrameAcc, doubleAcc. + * Look at the corresponding classes Vector Rotation Frame Twist and + * Wrench for the semantics of the methods. + * + * It also contains the 2nd derivative <-> RFrames.h + * + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id: frameacc.hpp 19905 2009-04-23 13:29:54Z ben2610 $ + * $Name: $ + ****************************************************************************/ + +#ifndef RRFRAMES_H +#define RRFRAMES_H + + +#include "utilities/rall2d.h" +#include "frames.hpp" + + + +namespace KDL { + +class TwistAcc; +typedef Rall2d<double,double,double> doubleAcc; + + +class VectorAcc +{ +public: + Vector p; //!< position vector + Vector v; //!< velocity vector + Vector dv; //!< acceleration vector +public: + VectorAcc():p(),v(),dv() {} + explicit VectorAcc(const Vector& _p):p(_p),v(Vector::Zero()),dv(Vector::Zero()) {} + VectorAcc(const Vector& _p,const Vector& _v):p(_p),v(_v),dv(Vector::Zero()) {} + VectorAcc(const Vector& _p,const Vector& _v,const Vector& _dv): + p(_p),v(_v),dv(_dv) {} + IMETHOD VectorAcc& operator = (const VectorAcc& arg); + IMETHOD VectorAcc& operator = (const Vector& arg); + IMETHOD VectorAcc& operator += (const VectorAcc& arg); + IMETHOD VectorAcc& operator -= (const VectorAcc& arg); + IMETHOD static VectorAcc Zero(); + IMETHOD void ReverseSign(); + IMETHOD doubleAcc Norm(); + IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2); + IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2); + IMETHOD friend VectorAcc operator + (const Vector& r1,const VectorAcc& r2); + IMETHOD friend VectorAcc operator - (const Vector& r1,const VectorAcc& r2); + IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const Vector& r2); + IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const Vector& r2); + IMETHOD friend VectorAcc operator * (const VectorAcc& r1,const VectorAcc& r2); + IMETHOD friend VectorAcc operator * (const VectorAcc& r1,const Vector& r2); + IMETHOD friend VectorAcc operator * (const Vector& r1,const VectorAcc& r2); + IMETHOD friend VectorAcc operator * (const VectorAcc& r1,double r2); + IMETHOD friend VectorAcc operator * (double r1,const VectorAcc& r2); + IMETHOD friend VectorAcc operator * (const doubleAcc& r1,const VectorAcc& r2); + IMETHOD friend VectorAcc operator * (const VectorAcc& r2,const doubleAcc& r1); + IMETHOD friend VectorAcc operator*(const Rotation& R,const VectorAcc& x); + + IMETHOD friend VectorAcc operator / (const VectorAcc& r1,double r2); + IMETHOD friend VectorAcc operator / (const VectorAcc& r2,const doubleAcc& r1); + + + IMETHOD friend bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps=epsilon); + IMETHOD friend bool Equal(const Vector& r1,const VectorAcc& r2,double eps=epsilon); + IMETHOD friend bool Equal(const VectorAcc& r1,const Vector& r2,double eps=epsilon); + IMETHOD friend VectorAcc operator - (const VectorAcc& r); + IMETHOD friend doubleAcc dot(const VectorAcc& lhs,const VectorAcc& rhs); + IMETHOD friend doubleAcc dot(const VectorAcc& lhs,const Vector& rhs); + IMETHOD friend doubleAcc dot(const Vector& lhs,const VectorAcc& rhs); +}; + + + +class RotationAcc +{ +public: + Rotation R; //!< rotation matrix + Vector w; //!< angular velocity vector + Vector dw; //!< angular acceration vector +public: + RotationAcc():R(),w() {} + explicit RotationAcc(const Rotation& _R):R(_R),w(Vector::Zero()){} + RotationAcc(const Rotation& _R,const Vector& _w,const Vector& _dw): + R(_R),w(_w),dw(_dw) {} + IMETHOD RotationAcc& operator = (const RotationAcc& arg); + IMETHOD RotationAcc& operator = (const Rotation& arg); + IMETHOD static RotationAcc Identity(); + IMETHOD RotationAcc Inverse() const; + IMETHOD VectorAcc Inverse(const VectorAcc& arg) const; + IMETHOD VectorAcc Inverse(const Vector& arg) const; + IMETHOD VectorAcc operator*(const VectorAcc& arg) const; + IMETHOD VectorAcc operator*(const Vector& arg) const; + + // Rotations + // The SetRot.. functions set the value of *this to the appropriate rotation matrix. + // The Rot... static functions give the value of the appropriate rotation matrix back. + // The DoRot... functions apply a rotation R to *this,such that *this = *this * R. + // IMETHOD void DoRotX(const doubleAcc& angle); + // IMETHOD void DoRotY(const doubleAcc& angle); + // IMETHOD void DoRotZ(const doubleAcc& angle); + // IMETHOD static RRotation RotX(const doubleAcc& angle); + // IMETHOD static RRotation RotY(const doubleAcc& angle); + // IMETHOD static RRotation RotZ(const doubleAcc& angle); + + // IMETHOD void SetRot(const Vector& rotaxis,const doubleAcc& angle); + // Along an arbitrary axes. The norm of rotvec is neglected. + // IMETHOD static RotationAcc Rot(const Vector& rotvec,const doubleAcc& angle); + // rotvec has arbitrary norm + // rotation around a constant vector ! + // IMETHOD static RotationAcc Rot2(const Vector& rotvec,const doubleAcc& angle); + // rotvec is normalized. + // rotation around a constant vector ! + + IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const RotationAcc& r2); + IMETHOD friend RotationAcc operator* (const Rotation& r1,const RotationAcc& r2); + IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const Rotation& r2); + IMETHOD friend bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps=epsilon); + IMETHOD friend bool Equal(const Rotation& r1,const RotationAcc& r2,double eps=epsilon); + IMETHOD friend bool Equal(const RotationAcc& r1,const Rotation& r2,double eps=epsilon); + IMETHOD TwistAcc Inverse(const TwistAcc& arg) const; + IMETHOD TwistAcc Inverse(const Twist& arg) const; + IMETHOD TwistAcc operator * (const TwistAcc& arg) const; + IMETHOD TwistAcc operator * (const Twist& arg) const; +}; + + + + +class FrameAcc +{ +public: + RotationAcc M; //!< Rotation,angular velocity, and angular acceleration of frame. + VectorAcc p; //!< Translation, velocity and acceleration of origin. +public: + FrameAcc(){} + explicit FrameAcc(const Frame& _T):M(_T.M),p(_T.p) {} + FrameAcc(const Frame& _T,const Twist& _t,const Twist& _dt): + M(_T.M,_t.rot,_dt.rot),p(_T.p,_t.vel,_dt.vel) {} + FrameAcc(const RotationAcc& _M,const VectorAcc& _p):M(_M),p(_p) {} + + IMETHOD FrameAcc& operator = (const FrameAcc& arg); + IMETHOD FrameAcc& operator = (const Frame& arg); + IMETHOD static FrameAcc Identity(); + IMETHOD FrameAcc Inverse() const; + IMETHOD VectorAcc Inverse(const VectorAcc& arg) const; + IMETHOD VectorAcc operator*(const VectorAcc& arg) const; + IMETHOD VectorAcc operator*(const Vector& arg) const; + IMETHOD VectorAcc Inverse(const Vector& arg) const; + IMETHOD Frame GetFrame() const; + IMETHOD Twist GetTwist() const; + IMETHOD Twist GetAccTwist() const; + IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const FrameAcc& f2); + IMETHOD friend FrameAcc operator * (const Frame& f1,const FrameAcc& f2); + IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const Frame& f2); + IMETHOD friend bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps=epsilon); + IMETHOD friend bool Equal(const Frame& r1,const FrameAcc& r2,double eps=epsilon); + IMETHOD friend bool Equal(const FrameAcc& r1,const Frame& r2,double eps=epsilon); + + IMETHOD TwistAcc Inverse(const TwistAcc& arg) const; + IMETHOD TwistAcc Inverse(const Twist& arg) const; + IMETHOD TwistAcc operator * (const TwistAcc& arg) const; + IMETHOD TwistAcc operator * (const Twist& arg) const; +}; + + + + + + + + +//very similar to Wrench class. +class TwistAcc +{ +public: + VectorAcc vel; //!< translational velocity and its 1st and 2nd derivative + VectorAcc rot; //!< rotational velocity and its 1st and 2nd derivative +public: + + TwistAcc():vel(),rot() {}; + TwistAcc(const VectorAcc& _vel,const VectorAcc& _rot):vel(_vel),rot(_rot) {}; + + IMETHOD TwistAcc& operator-=(const TwistAcc& arg); + IMETHOD TwistAcc& operator+=(const TwistAcc& arg); + + IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,double rhs); + IMETHOD friend TwistAcc operator*(double lhs,const TwistAcc& rhs); + IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,double rhs); + + IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,const doubleAcc& rhs); + IMETHOD friend TwistAcc operator*(const doubleAcc& lhs,const TwistAcc& rhs); + IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,const doubleAcc& rhs); + + IMETHOD friend TwistAcc operator+(const TwistAcc& lhs,const TwistAcc& rhs); + IMETHOD friend TwistAcc operator-(const TwistAcc& lhs,const TwistAcc& rhs); + IMETHOD friend TwistAcc operator-(const TwistAcc& arg); + + IMETHOD friend void SetToZero(TwistAcc& v); + + static IMETHOD TwistAcc Zero(); + + IMETHOD void ReverseSign(); + + IMETHOD TwistAcc RefPoint(const VectorAcc& v_base_AB); + // Changes the reference point of the RTwist. + // The RVector v_base_AB is expressed in the same base as the RTwist + // The RVector v_base_AB is a RVector from the old point to + // the new point. + // Complexity : 6M+6A + + IMETHOD friend bool Equal(const TwistAcc& a,const TwistAcc& b,double eps=epsilon); + IMETHOD friend bool Equal(const Twist& a,const TwistAcc& b,double eps=epsilon); + IMETHOD friend bool Equal(const TwistAcc& a,const Twist& b,double eps=epsilon); + + + IMETHOD Twist GetTwist() const; + IMETHOD Twist GetTwistDot() const; + + friend class RotationAcc; + friend class FrameAcc; + +}; + + + + + + + +#ifdef KDL_INLINE +#include "frameacc.inl" +#endif + +} + + + + + +#endif diff --git a/intern/itasc/kdl/frameacc.inl b/intern/itasc/kdl/frameacc.inl new file mode 100644 index 00000000000..a8ea35ad436 --- /dev/null +++ b/intern/itasc/kdl/frameacc.inl @@ -0,0 +1,598 @@ +/***************************************************************************** + * \file + * provides inline functions of rrframes.h + * + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id: frameacc.inl 19905 2009-04-23 13:29:54Z ben2610 $ + * $Name: $ + ****************************************************************************/ + + + + +/////////////////// VectorAcc ///////////////////////////////////// + +VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2) { + return VectorAcc(r1.p+r2.p,r1.v+r2.v,r1.dv+r2.dv); +} + +VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2) { + return VectorAcc(r1.p-r2.p, r1.v-r2.v, r1.dv-r2.dv); +} +VectorAcc operator + (const Vector& r1,const VectorAcc& r2) { + return VectorAcc(r1+r2.p,r2.v,r2.dv); +} + +VectorAcc operator - (const Vector& r1,const VectorAcc& r2) { + return VectorAcc(r1-r2.p, -r2.v, -r2.dv); +} +VectorAcc operator + (const VectorAcc& r1,const Vector& r2) { + return VectorAcc(r1.p+r2,r1.v,r1.dv); +} + +VectorAcc operator - (const VectorAcc& r1,const Vector& r2) { + return VectorAcc(r1.p-r2, r1.v, r1.dv); +} + +// unary - +VectorAcc operator - (const VectorAcc& r) { + return VectorAcc(-r.p,-r.v,-r.dv); +} + +// cross prod. +VectorAcc operator * (const VectorAcc& r1,const VectorAcc& r2) { + return VectorAcc(r1.p*r2.p, + r1.p*r2.v+r1.v*r2.p, + r1.dv*r2.p+2*r1.v*r2.v+r1.p*r2.dv + ); +} + +VectorAcc operator * (const VectorAcc& r1,const Vector& r2) { + return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 ); +} + +VectorAcc operator * (const Vector& r1,const VectorAcc& r2) { + return VectorAcc(r1*r2.p, r1*r2.v, r1*r2.dv ); +} + + + +// scalar mult. +VectorAcc operator * (double r1,const VectorAcc& r2) { + return VectorAcc(r1*r2.p, r1*r2.v, r1*r2.dv ); +} + +VectorAcc operator * (const VectorAcc& r1,double r2) { + return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 ); +} + +VectorAcc operator * (const doubleAcc& r1,const VectorAcc& r2) { + return VectorAcc(r1.t*r2.p, + r1.t*r2.v + r1.d*r2.p, + r1.t*r2.dv + 2*r1.d*r2.v + r1.dd*r2.p + ); +} + +VectorAcc operator * (const VectorAcc& r2,const doubleAcc& r1) { + return VectorAcc(r1.t*r2.p, + r1.t*r2.v + r1.d*r2.p, + r1.t*r2.dv + 2*r1.d*r2.v + r1.dd*r2.p + ); +} + +VectorAcc& VectorAcc::operator = (const VectorAcc& arg) { + p=arg.p; + v=arg.v; + dv=arg.dv; + return *this; +} + +VectorAcc& VectorAcc::operator = (const Vector& arg) { + p=arg; + v=Vector::Zero(); + dv=Vector::Zero(); + return *this; +} + +VectorAcc& VectorAcc::operator += (const VectorAcc& arg) { + p+=arg.p; + v+=arg.v; + dv+= arg.dv; + return *this; +} +VectorAcc& VectorAcc::operator -= (const VectorAcc& arg) { + p-=arg.p; + v-=arg.v; + dv-=arg.dv; + return *this; +} + +VectorAcc VectorAcc::Zero() { + return VectorAcc(Vector::Zero(),Vector::Zero(),Vector::Zero()); +} + +void VectorAcc::ReverseSign() { + p.ReverseSign(); + v.ReverseSign(); + dv.ReverseSign(); +} + +doubleAcc VectorAcc::Norm() { + doubleAcc res; + res.t = p.Norm(); + res.d = dot(p,v)/res.t; + res.dd = (dot(p,dv)+dot(v,v)-res.d*res.d)/res.t; + return res; +} + +doubleAcc dot(const VectorAcc& lhs,const VectorAcc& rhs) { + return doubleAcc( dot(lhs.p,rhs.p), + dot(lhs.p,rhs.v)+dot(lhs.v,rhs.p), + dot(lhs.p,rhs.dv)+2*dot(lhs.v,rhs.v)+dot(lhs.dv,rhs.p) + ); +} + +doubleAcc dot(const VectorAcc& lhs,const Vector& rhs) { + return doubleAcc( dot(lhs.p,rhs), + dot(lhs.v,rhs), + dot(lhs.dv,rhs) + ); +} + +doubleAcc dot(const Vector& lhs,const VectorAcc& rhs) { + return doubleAcc( dot(lhs,rhs.p), + dot(lhs,rhs.v), + dot(lhs,rhs.dv) + ); +} + + +bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps) { + return (Equal(r1.p,r2.p,eps) + && Equal(r1.v,r2.v,eps) + && Equal(r1.dv,r2.dv,eps) + ); +} + +bool Equal(const Vector& r1,const VectorAcc& r2,double eps) { + return (Equal(r1,r2.p,eps) + && Equal(Vector::Zero(),r2.v,eps) + && Equal(Vector::Zero(),r2.dv,eps) + ); +} + +bool Equal(const VectorAcc& r1,const Vector& r2,double eps) { + return (Equal(r1.p,r2,eps) + && Equal(r1.v,Vector::Zero(),eps) + && Equal(r1.dv,Vector::Zero(),eps) + ); +} + +VectorAcc operator / (const VectorAcc& r1,double r2) { + return r1*(1.0/r2); +} + +VectorAcc operator / (const VectorAcc& r2,const doubleAcc& r1) { + return r2*(1.0/r1); +} + + + +/////////////////// RotationAcc ///////////////////////////////////// + +RotationAcc operator* (const RotationAcc& r1,const RotationAcc& r2) { + return RotationAcc( r1.R * r2.R, + r1.w + r1.R*r2.w, + r1.dw + r1.w*(r1.R*r2.w) + r1.R*r2.dw + ); +} + +RotationAcc operator* (const Rotation& r1,const RotationAcc& r2) { + return RotationAcc( r1*r2.R, r1*r2.w, r1*r2.dw); +} + +RotationAcc operator* (const RotationAcc& r1,const Rotation& r2) { + return RotationAcc( r1.R*r2, r1.w, r1.dw ); +} + +RotationAcc& RotationAcc::operator = (const RotationAcc& arg) { + R=arg.R; + w=arg.w; + dw=arg.dw; + return *this; +} +RotationAcc& RotationAcc::operator = (const Rotation& arg) { + R = arg; + w = Vector::Zero(); + dw = Vector::Zero(); + return *this; +} + +RotationAcc RotationAcc::Identity() { + return RotationAcc(Rotation::Identity(),Vector::Zero(),Vector::Zero()); +} + +RotationAcc RotationAcc::Inverse() const { + return RotationAcc(R.Inverse(),-R.Inverse(w),-R.Inverse(dw)); +} + +VectorAcc RotationAcc::Inverse(const VectorAcc& arg) const { + VectorAcc tmp; + tmp.p = R.Inverse(arg.p); + tmp.v = R.Inverse(arg.v - w * arg.p); + tmp.dv = R.Inverse(arg.dv - dw*arg.p - w*(arg.v+R*tmp.v)); + return tmp; +} + +VectorAcc RotationAcc::Inverse(const Vector& arg) const { + VectorAcc tmp; + tmp.p = R.Inverse(arg); + tmp.v = R.Inverse(-w*arg); + tmp.dv = R.Inverse(-dw*arg - w*(R*tmp.v)); + return tmp; +} + + +VectorAcc RotationAcc::operator*(const VectorAcc& arg) const { + VectorAcc tmp; + tmp.p = R*arg.p; + tmp.dv = R*arg.v; + tmp.v = w*tmp.p + tmp.dv; + tmp.dv = dw*tmp.p + w*(tmp.v + tmp.dv) + R*arg.dv; + return tmp; +} + +VectorAcc operator*(const Rotation& R,const VectorAcc& x) { + return VectorAcc(R*x.p,R*x.v,R*x.dv); +} + +VectorAcc RotationAcc::operator*(const Vector& arg) const { + VectorAcc tmp; + tmp.p = R*arg; + tmp.v = w*tmp.p; + tmp.dv = dw*tmp.p + w*tmp.v; + return tmp; +} + +/* + // = Rotations + // The Rot... static functions give the value of the appropriate rotation matrix back. + // The DoRot... functions apply a rotation R to *this,such that *this = *this * R. + + void RRotation::DoRotX(const RDouble& angle) { + w+=R*Vector(angle.grad,0,0); + R.DoRotX(angle.t); + } +RotationAcc RotationAcc::RotX(const doubleAcc& angle) { + return RotationAcc(Rotation::RotX(angle.t), + Vector(angle.d,0,0), + Vector(angle.dd,0,0) + ); +} + + void RRotation::DoRotY(const RDouble& angle) { + w+=R*Vector(0,angle.grad,0); + R.DoRotY(angle.t); + } +RotationAcc RotationAcc::RotY(const doubleAcc& angle) { + return RotationAcc( + Rotation::RotX(angle.t), + Vector(0,angle.d,0), + Vector(0,angle.dd,0) + ); +} + + void RRotation::DoRotZ(const RDouble& angle) { + w+=R*Vector(0,0,angle.grad); + R.DoRotZ(angle.t); + } +RotationAcc RotationAcc::RotZ(const doubleAcc& angle) { + return RotationAcc( + Rotation::RotZ(angle.t), + Vector(0,0,angle.d), + Vector(0,0,angle.dd) + ); +} + + + RRotation RRotation::Rot(const Vector& rotvec,const RDouble& angle) + // rotvec has arbitrary norm + // rotation around a constant vector ! + { + Vector v = rotvec.Normalize(); + return RRotation(Rotation::Rot2(v,angle.t),v*angle.grad); + } + + RRotation RRotation::Rot2(const Vector& rotvec,const RDouble& angle) + // rotvec is normalized. + { + return RRotation(Rotation::Rot2(rotvec,angle.t),rotvec*angle.grad); + } + +*/ + +bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps) { + return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps) && Equal(r1.dw,r2.dw,eps) ); +} +bool Equal(const Rotation& r1,const RotationAcc& r2,double eps) { + return (Equal(Vector::Zero(),r2.w,eps) && Equal(r1,r2.R,eps) && + Equal(Vector::Zero(),r2.dw,eps) ); +} +bool Equal(const RotationAcc& r1,const Rotation& r2,double eps) { + return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps) && + Equal(r1.dw,Vector::Zero(),eps) ); +} + + +// Methods and operators related to FrameAcc +// They all delegate most of the work to RotationAcc and VectorAcc +FrameAcc& FrameAcc::operator = (const FrameAcc& arg) { + M=arg.M; + p=arg.p; + return *this; +} + +FrameAcc FrameAcc::Identity() { + return FrameAcc(RotationAcc::Identity(),VectorAcc::Zero()); +} + + +FrameAcc operator *(const FrameAcc& lhs,const FrameAcc& rhs) +{ + return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); +} +FrameAcc operator *(const FrameAcc& lhs,const Frame& rhs) +{ + return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); +} +FrameAcc operator *(const Frame& lhs,const FrameAcc& rhs) +{ + return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); +} + +VectorAcc FrameAcc::operator *(const VectorAcc & arg) const +{ + return M*arg+p; +} +VectorAcc FrameAcc::operator *(const Vector & arg) const +{ + return M*arg+p; +} + +VectorAcc FrameAcc::Inverse(const VectorAcc& arg) const +{ + return M.Inverse(arg-p); +} + +VectorAcc FrameAcc::Inverse(const Vector& arg) const +{ + return M.Inverse(arg-p); +} + +FrameAcc FrameAcc::Inverse() const +{ + return FrameAcc(M.Inverse(),-M.Inverse(p)); +} + +FrameAcc& FrameAcc::operator =(const Frame & arg) +{ + M = arg.M; + p = arg.p; + return *this; +} + +bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps) { + return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); +} +bool Equal(const Frame& r1,const FrameAcc& r2,double eps) { + return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); +} +bool Equal(const FrameAcc& r1,const Frame& r2,double eps) { + return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); +} + + +Frame FrameAcc::GetFrame() const { + return Frame(M.R,p.p); +} + + +Twist FrameAcc::GetTwist() const { + return Twist(p.v,M.w); +} + + +Twist FrameAcc::GetAccTwist() const { + return Twist(p.dv,M.dw); +} + + + + + + + + + + + + + + + + + +TwistAcc TwistAcc::Zero() +{ + return TwistAcc(VectorAcc::Zero(),VectorAcc::Zero()); +} + + +void TwistAcc::ReverseSign() +{ + vel.ReverseSign(); + rot.ReverseSign(); +} + +TwistAcc TwistAcc::RefPoint(const VectorAcc& v_base_AB) + // Changes the reference point of the TwistAcc. + // The RVector v_base_AB is expressed in the same base as the TwistAcc + // The RVector v_base_AB is a RVector from the old point to + // the new point. + // Complexity : 6M+6A +{ + return TwistAcc(this->vel+this->rot*v_base_AB,this->rot); +} + +TwistAcc& TwistAcc::operator-=(const TwistAcc& arg) +{ + vel-=arg.vel; + rot -=arg.rot; + return *this; +} + +TwistAcc& TwistAcc::operator+=(const TwistAcc& arg) +{ + vel+=arg.vel; + rot +=arg.rot; + return *this; +} + + +TwistAcc operator*(const TwistAcc& lhs,double rhs) +{ + return TwistAcc(lhs.vel*rhs,lhs.rot*rhs); +} + +TwistAcc operator*(double lhs,const TwistAcc& rhs) +{ + return TwistAcc(lhs*rhs.vel,lhs*rhs.rot); +} + +TwistAcc operator/(const TwistAcc& lhs,double rhs) +{ + return TwistAcc(lhs.vel/rhs,lhs.rot/rhs); +} + + +TwistAcc operator*(const TwistAcc& lhs,const doubleAcc& rhs) +{ + return TwistAcc(lhs.vel*rhs,lhs.rot*rhs); +} + +TwistAcc operator*(const doubleAcc& lhs,const TwistAcc& rhs) +{ + return TwistAcc(lhs*rhs.vel,lhs*rhs.rot); +} + +TwistAcc operator/(const TwistAcc& lhs,const doubleAcc& rhs) +{ + return TwistAcc(lhs.vel/rhs,lhs.rot/rhs); +} + + + +// addition of TwistAcc's +TwistAcc operator+(const TwistAcc& lhs,const TwistAcc& rhs) +{ + return TwistAcc(lhs.vel+rhs.vel,lhs.rot+rhs.rot); +} + +TwistAcc operator-(const TwistAcc& lhs,const TwistAcc& rhs) +{ + return TwistAcc(lhs.vel-rhs.vel,lhs.rot-rhs.rot); +} + +// unary - +TwistAcc operator-(const TwistAcc& arg) +{ + return TwistAcc(-arg.vel,-arg.rot); +} + + + + + +TwistAcc RotationAcc::Inverse(const TwistAcc& arg) const +{ + return TwistAcc(Inverse(arg.vel),Inverse(arg.rot)); +} + +TwistAcc RotationAcc::operator * (const TwistAcc& arg) const +{ + return TwistAcc((*this)*arg.vel,(*this)*arg.rot); +} + +TwistAcc RotationAcc::Inverse(const Twist& arg) const +{ + return TwistAcc(Inverse(arg.vel),Inverse(arg.rot)); +} + +TwistAcc RotationAcc::operator * (const Twist& arg) const +{ + return TwistAcc((*this)*arg.vel,(*this)*arg.rot); +} + + +TwistAcc FrameAcc::operator * (const TwistAcc& arg) const +{ + TwistAcc tmp; + tmp.rot = M*arg.rot; + tmp.vel = M*arg.vel+p*tmp.rot; + return tmp; +} + +TwistAcc FrameAcc::operator * (const Twist& arg) const +{ + TwistAcc tmp; + tmp.rot = M*arg.rot; + tmp.vel = M*arg.vel+p*tmp.rot; + return tmp; +} + +TwistAcc FrameAcc::Inverse(const TwistAcc& arg) const +{ + TwistAcc tmp; + tmp.rot = M.Inverse(arg.rot); + tmp.vel = M.Inverse(arg.vel-p*arg.rot); + return tmp; +} + +TwistAcc FrameAcc::Inverse(const Twist& arg) const +{ + TwistAcc tmp; + tmp.rot = M.Inverse(arg.rot); + tmp.vel = M.Inverse(arg.vel-p*arg.rot); + return tmp; +} + +Twist TwistAcc::GetTwist() const { + return Twist(vel.p,rot.p); +} + +Twist TwistAcc::GetTwistDot() const { + return Twist(vel.v,rot.v); +} + +bool Equal(const TwistAcc& a,const TwistAcc& b,double eps) { + return (Equal(a.rot,b.rot,eps)&& + Equal(a.vel,b.vel,eps) ); +} +bool Equal(const Twist& a,const TwistAcc& b,double eps) { + return (Equal(a.rot,b.rot,eps)&& + Equal(a.vel,b.vel,eps) ); +} +bool Equal(const TwistAcc& a,const Twist& b,double eps) { + return (Equal(a.rot,b.rot,eps)&& + Equal(a.vel,b.vel,eps) ); +} + diff --git a/intern/itasc/kdl/frames.cpp b/intern/itasc/kdl/frames.cpp new file mode 100644 index 00000000000..7dcc39f2cd4 --- /dev/null +++ b/intern/itasc/kdl/frames.cpp @@ -0,0 +1,389 @@ +/*************************************************************************** + frames.cxx - description + ------------------------- + begin : June 2006 + copyright : (C) 2006 Erwin Aertbelien + email : firstname.lastname@mech.kuleuven.ac.be + + History (only major changes)( AUTHOR-Description ) : + + *************************************************************************** + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Lesser General Public * + * License as published by the Free Software Foundation; either * + * version 2.1 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with this library; if not, write to the Free Software * + * Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307 USA * + * * + ***************************************************************************/ + +#include "frames.hpp" + +namespace KDL { + +#ifndef KDL_INLINE +#include "frames.inl" +#endif + +void Frame::Make4x4(double * d) +{ + int i; + int j; + for (i=0;i<3;i++) { + for (j=0;j<3;j++) + d[i*4+j]=M(i,j); + d[i*4+3] = p(i)/1000; + } + for (j=0;j<3;j++) + d[12+j] = 0.; + d[15] = 1; +} + +Frame Frame::DH_Craig1989(double a,double alpha,double d,double theta) +// returns Modified Denavit-Hartenberg parameters (According to Craig) +{ + double ct,st,ca,sa; + ct = cos(theta); + st = sin(theta); + sa = sin(alpha); + ca = cos(alpha); + return Frame(Rotation( + ct, -st, 0, + st*ca, ct*ca, -sa, + st*sa, ct*sa, ca ), + Vector( + a, -sa*d, ca*d ) + ); +} + +Frame Frame::DH(double a,double alpha,double d,double theta) +// returns Denavit-Hartenberg parameters (Non-Modified DH) +{ + double ct,st,ca,sa; + ct = cos(theta); + st = sin(theta); + sa = sin(alpha); + ca = cos(alpha); + return Frame(Rotation( + ct, -st*ca, st*sa, + st, ct*ca, -ct*sa, + 0, sa, ca ), + Vector( + a*ct, a*st, d ) + ); +} + +double Vector2::Norm() const +{ + double tmp0 = fabs(data[0]); + double tmp1 = fabs(data[1]); + if (tmp0 >= tmp1) { + if (tmp1 == 0) + return 0; + return tmp0*sqrt(1+sqr(tmp1/tmp0)); + } else { + return tmp1*sqrt(1+sqr(tmp0/tmp1)); + } +} +// makes v a unitvector and returns the norm of v. +// if v is smaller than eps, Vector(1,0,0) is returned with norm 0. +// if this is not good, check the return value of this method. +double Vector2::Normalize(double eps) { + double v = this->Norm(); + if (v < eps) { + *this = Vector2(1,0); + return v; + } else { + *this = (*this)/v; + return v; + } +} + + +// do some effort not to lose precision +double Vector::Norm() const +{ + double tmp1; + double tmp2; + tmp1 = fabs(data[0]); + tmp2 = fabs(data[1]); + if (tmp1 >= tmp2) { + tmp2=fabs(data[2]); + if (tmp1 >= tmp2) { + if (tmp1 == 0) { + // only to everything exactly zero case, all other are handled correctly + return 0; + } + return tmp1*sqrt(1+sqr(data[1]/data[0])+sqr(data[2]/data[0])); + } else { + return tmp2*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2])); + } + } else { + tmp1=fabs(data[2]); + if (tmp2 > tmp1) { + return tmp2*sqrt(1+sqr(data[0]/data[1])+sqr(data[2]/data[1])); + } else { + return tmp1*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2])); + } + } +} + +// makes v a unitvector and returns the norm of v. +// if v is smaller than eps, Vector(1,0,0) is returned with norm 0. +// if this is not good, check the return value of this method. +double Vector::Normalize(double eps) { + double v = this->Norm(); + if (v < eps) { + *this = Vector(1,0,0); + return v; + } else { + *this = (*this)/v; + return v; + } +} + + +bool Equal(const Rotation& a,const Rotation& b,double eps) { + return (Equal(a.data[0],b.data[0],eps) && + Equal(a.data[1],b.data[1],eps) && + Equal(a.data[2],b.data[2],eps) && + Equal(a.data[3],b.data[3],eps) && + Equal(a.data[4],b.data[4],eps) && + Equal(a.data[5],b.data[5],eps) && + Equal(a.data[6],b.data[6],eps) && + Equal(a.data[7],b.data[7],eps) && + Equal(a.data[8],b.data[8],eps) ); +} + +void Rotation::Ortho() +{ + double n; + n=sqrt(sqr(data[0])+sqr(data[3])+sqr(data[6]));n=(n>1e-10)?1.0/n:0.0;data[0]*=n;data[3]*=n;data[6]*=n; + n=sqrt(sqr(data[1])+sqr(data[4])+sqr(data[7]));n=(n>1e-10)?1.0/n:0.0;data[1]*=n;data[4]*=n;data[7]*=n; + n=sqrt(sqr(data[2])+sqr(data[5])+sqr(data[8]));n=(n>1e-10)?1.0/n:0.0;data[2]*=n;data[5]*=n;data[8]*=n; +} + +Rotation operator *(const Rotation& lhs,const Rotation& rhs) +// Complexity : 27M+27A +{ + return Rotation( + lhs.data[0]*rhs.data[0]+lhs.data[1]*rhs.data[3]+lhs.data[2]*rhs.data[6], + lhs.data[0]*rhs.data[1]+lhs.data[1]*rhs.data[4]+lhs.data[2]*rhs.data[7], + lhs.data[0]*rhs.data[2]+lhs.data[1]*rhs.data[5]+lhs.data[2]*rhs.data[8], + lhs.data[3]*rhs.data[0]+lhs.data[4]*rhs.data[3]+lhs.data[5]*rhs.data[6], + lhs.data[3]*rhs.data[1]+lhs.data[4]*rhs.data[4]+lhs.data[5]*rhs.data[7], + lhs.data[3]*rhs.data[2]+lhs.data[4]*rhs.data[5]+lhs.data[5]*rhs.data[8], + lhs.data[6]*rhs.data[0]+lhs.data[7]*rhs.data[3]+lhs.data[8]*rhs.data[6], + lhs.data[6]*rhs.data[1]+lhs.data[7]*rhs.data[4]+lhs.data[8]*rhs.data[7], + lhs.data[6]*rhs.data[2]+lhs.data[7]*rhs.data[5]+lhs.data[8]*rhs.data[8] + ); + +} + + +Rotation Rotation::RPY(double roll,double pitch,double yaw) + { + double ca1,cb1,cc1,sa1,sb1,sc1; + ca1 = cos(yaw); sa1 = sin(yaw); + cb1 = cos(pitch);sb1 = sin(pitch); + cc1 = cos(roll);sc1 = sin(roll); + return Rotation(ca1*cb1,ca1*sb1*sc1 - sa1*cc1,ca1*sb1*cc1 + sa1*sc1, + sa1*cb1,sa1*sb1*sc1 + ca1*cc1,sa1*sb1*cc1 - ca1*sc1, + -sb1,cb1*sc1,cb1*cc1); + } + +// Gives back a rotation matrix specified with RPY convention +void Rotation::GetRPY(double& roll,double& pitch,double& yaw) const + { + if (fabs(data[6]) > 1.0 - epsilon ) { + roll = -sign(data[6]) * atan2(data[1], data[4]); + pitch= -sign(data[6]) * PI / 2; + yaw = 0.0 ; + } else { + roll = atan2(data[7], data[8]); + pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) ) ); + yaw = atan2(data[3], data[0]); + } + } + +Rotation Rotation::EulerZYZ(double Alfa,double Beta,double Gamma) { + double sa,ca,sb,cb,sg,cg; + sa = sin(Alfa);ca = cos(Alfa); + sb = sin(Beta);cb = cos(Beta); + sg = sin(Gamma);cg = cos(Gamma); + return Rotation( ca*cb*cg-sa*sg, -ca*cb*sg-sa*cg, ca*sb, + sa*cb*cg+ca*sg, -sa*cb*sg+ca*cg, sa*sb, + -sb*cg , sb*sg, cb + ); + + } + + +void Rotation::GetEulerZYZ(double& alfa,double& beta,double& gamma) const { + if (fabs(data[6]) < epsilon ) { + alfa=0.0; + if (data[8]>0) { + beta = 0.0; + gamma= atan2(-data[1],data[0]); + } else { + beta = PI; + gamma= atan2(data[1],-data[0]); + } + } else { + alfa=atan2(data[5], data[2]); + beta=atan2(sqrt( sqr(data[6]) +sqr(data[7]) ),data[8]); + gamma=atan2(data[7], -data[6]); + } + } + +Rotation Rotation::Rot(const Vector& rotaxis,double angle) { + // The formula is + // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr)) + // can be found by multiplying it with an arbitrary vector p + // and noting that this vector is rotated. + double ct = cos(angle); + double st = sin(angle); + double vt = 1-ct; + Vector rotvec = rotaxis; + rotvec.Normalize(); + return Rotation( + ct + vt*rotvec(0)*rotvec(0), + -rotvec(2)*st + vt*rotvec(0)*rotvec(1), + rotvec(1)*st + vt*rotvec(0)*rotvec(2), + rotvec(2)*st + vt*rotvec(1)*rotvec(0), + ct + vt*rotvec(1)*rotvec(1), + -rotvec(0)*st + vt*rotvec(1)*rotvec(2), + -rotvec(1)*st + vt*rotvec(2)*rotvec(0), + rotvec(0)*st + vt*rotvec(2)*rotvec(1), + ct + vt*rotvec(2)*rotvec(2) + ); + } + +Rotation Rotation::Rot2(const Vector& rotvec,double angle) { + // rotvec should be normalized ! + // The formula is + // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr)) + // can be found by multiplying it with an arbitrary vector p + // and noting that this vector is rotated. + double ct = cos(angle); + double st = sin(angle); + double vt = 1-ct; + return Rotation( + ct + vt*rotvec(0)*rotvec(0), + -rotvec(2)*st + vt*rotvec(0)*rotvec(1), + rotvec(1)*st + vt*rotvec(0)*rotvec(2), + rotvec(2)*st + vt*rotvec(1)*rotvec(0), + ct + vt*rotvec(1)*rotvec(1), + -rotvec(0)*st + vt*rotvec(1)*rotvec(2), + -rotvec(1)*st + vt*rotvec(2)*rotvec(0), + rotvec(0)*st + vt*rotvec(2)*rotvec(1), + ct + vt*rotvec(2)*rotvec(2) + ); +} + + + +Vector Rotation::GetRot() const + // Returns a vector with the direction of the equiv. axis + // and its norm is angle + { + Vector axis = Vector((data[7]-data[5]), + (data[2]-data[6]), + (data[3]-data[1]) )/2; + + double sa = axis.Norm(); + double ca = (data[0]+data[4]+data[8]-1)/2.0; + double alfa; + if (sa > epsilon) + alfa = ::atan2(sa,ca)/sa; + else { + if (ca < 0.0) { + alfa = KDL::PI; + axis.data[0] = 0.0; + axis.data[1] = 0.0; + axis.data[2] = 0.0; + if (data[0] > 0.0) { + axis.data[0] = 1.0; + } else if (data[4] > 0.0) { + axis.data[1] = 1.0; + } else { + axis.data[2] = 1.0; + } + } else { + alfa = 0.0; + } + } + return axis * alfa; + } + +Vector2 Rotation::GetXZRot() const +{ + // [0,1,0] x Y + Vector2 axis(data[7], -data[1]); + double norm = axis.Normalize(); + if (norm < epsilon) { + norm = (data[4] < 0.0) ? PI : 0.0; + } else { + norm = acos(data[4]); + } + return axis*norm; +} + + +/** Returns the rotation angle around the equiv. axis + * @param axis the rotation axis is returned in this variable + * @param eps : in the case of angle == 0 : rot axis is undefined and choosen + * to be +/- Z-axis + * in the case of angle == PI : 2 solutions, positive Z-component + * of the axis is choosen. + * @result returns the rotation angle (between [0..PI] ) + * /todo : + * Check corresponding routines in rframes and rrframes + */ +double Rotation::GetRotAngle(Vector& axis,double eps) const { + double ca = (data[0]+data[4]+data[8]-1)/2.0; + if (ca>1-eps) { + // undefined choose the Z-axis, and angle 0 + axis = Vector(0,0,1); + return 0; + } + if (ca < -1+eps) { + // two solutions, choose a positive Z-component of the axis + double z = sqrt( (data[8]+1)/2 ); + double x = (data[2])/2/z; + double y = (data[5])/2/z; + axis = Vector( x,y,z ); + return PI; + } + double angle = acos(ca); + double sa = sin(angle); + axis = Vector((data[7]-data[5])/2/sa, + (data[2]-data[6])/2/sa, + (data[3]-data[1])/2/sa ); + return angle; +} + +bool operator==(const Rotation& a,const Rotation& b) { +#ifdef KDL_USE_EQUAL + return Equal(a,b); +#else + return ( a.data[0]==b.data[0] && + a.data[1]==b.data[1] && + a.data[2]==b.data[2] && + a.data[3]==b.data[3] && + a.data[4]==b.data[4] && + a.data[5]==b.data[5] && + a.data[6]==b.data[6] && + a.data[7]==b.data[7] && + a.data[8]==b.data[8] ); +#endif +} +} diff --git a/intern/itasc/kdl/frames.hpp b/intern/itasc/kdl/frames.hpp new file mode 100644 index 00000000000..20590c5303e --- /dev/null +++ b/intern/itasc/kdl/frames.hpp @@ -0,0 +1,1097 @@ +/*************************************************************************** + frames.hpp `- description + ------------------------- + begin : June 2006 + copyright : (C) 2006 Erwin Aertbelien + email : firstname.lastname@mech.kuleuven.be + + History (only major changes)( AUTHOR-Description ) : + + *************************************************************************** + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Lesser General Public * + * License as published by the Free Software Foundation; either * + * version 2.1 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with this library; if not, write to the Free Software * + * Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307 USA * + * * + ***************************************************************************/ + +/** + * \file + * \warning + * Efficienty can be improved by writing p2 = A*(B*(C*p1))) instead of + * p2=A*B*C*p1 + * + * \par PROPOSED NAMING CONVENTION FOR FRAME-like OBJECTS + * + * \verbatim + * A naming convention of objects of the type defined in this file : + * (1) Frame : F... + * Rotation : R ... + * (2) Twist : T ... + * Wrench : W ... + * Vector : V ... + * This prefix is followed by : + * for category (1) : + * F_A_B : w.r.t. frame A, frame B expressed + * ( each column of F_A_B corresponds to an axis of B, + * expressed w.r.t. frame A ) + * in mathematical convention : + * A + * F_A_B == F + * B + * + * for category (2) : + * V_B : a vector expressed w.r.t. frame B + * + * This can also be prepended by a name : + * e.g. : temporaryV_B + * + * With this convention one can write : + * + * F_A_B = F_B_A.Inverse(); + * F_A_C = F_A_B * F_B_C; + * V_B = F_B_C * V_C; // both translation and rotation + * V_B = R_B_C * V_C; // only rotation + * \endverbatim + * + * \par CONVENTIONS FOR WHEN USED WITH ROBOTS : + * + * \verbatim + * world : represents the frame ([1 0 0,0 1 0,0 0 1],[0 0 0]') + * mp : represents mounting plate of a robot + * (i.e. everything before MP is constructed by robot manufacturer + * everything after MP is tool ) + * tf : represents task frame of a robot + * (i.e. frame in which motion and force control is expressed) + * sf : represents sensor frame of a robot + * (i.e. frame at which the forces measured by the force sensor + * are expressed ) + * + * Frame F_world_mp=...; + * Frame F_mp_sf(..) + * Frame F_mp_tf(,.) + * + * Wrench are measured in sensor frame SF, so one could write : + * Wrench_tf = F_mp_tf.Inverse()* ( F_mp_sf * Wrench_sf ); + * \endverbatim + * + * \par CONVENTIONS REGARDING UNITS : + * Any consistent series of units can be used, e.g. N,mm,Nmm,..mm/sec + * + * \par Twist and Wrench transformations + * 3 different types of transformations do exist for the twists + * and wrenches. + * + * \verbatim + * 1) Frame * Twist or Frame * Wrench : + * this transforms both the velocity/force reference point + * and the basis to which the twist/wrench are expressed. + * 2) Rotation * Twist or Rotation * Wrench : + * this transforms the basis to which the twist/wrench are + * expressed, but leaves the reference point intact. + * 3) Twist.RefPoint(v_base_AB) or Wrench.RefPoint(v_base_AB) + * this transforms only the reference point. v is expressed + * in the same base as the twist/wrench and points from the + * old reference point to the new reference point. + * \endverbatim + * + * \par Complexity + * Sometimes the amount of work is given in the documentation + * e.g. 6M+3A means 6 multiplications and 3 additions. + * + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + ****************************************************************************/ +#ifndef KDL_FRAMES_H +#define KDL_FRAMES_H + + +#include "utilities/kdl-config.h" +#include "utilities/utility.h" + +///////////////////////////////////////////////////////////// + +namespace KDL { + + + +class Vector; +class Rotation; +class Frame; +class Wrench; +class Twist; +class Vector2; +class Rotation2; +class Frame2; + + + +/** + * \brief A concrete implementation of a 3 dimensional vector class + */ +class Vector +{ +public: + double data[3]; + //! Does not initialise the Vector to zero. use Vector::Zero() or SetToZero for that + inline Vector() {data[0]=data[1]=data[2] = 0.0;} + + //! Constructs a vector out of the three values x, y and z + inline Vector(double x,double y, double z); + + //! Constructs a vector out of an array of three values x, y and z + inline Vector(double* xyz); + + //! Constructs a vector out of an array of three values x, y and z + inline Vector(float* xyz); + + //! Assignment operator. The normal copy by value semantics. + inline Vector(const Vector& arg); + + //! store vector components in array + inline void GetValue(double* xyz) const; + + //! Assignment operator. The normal copy by value semantics. + inline Vector& operator = ( const Vector& arg); + + //! Access to elements, range checked when NDEBUG is not set, from 0..2 + inline double operator()(int index) const; + + //! Access to elements, range checked when NDEBUG is not set, from 0..2 + inline double& operator() (int index); + + //! Equivalent to double operator()(int index) const + double operator[] ( int index ) const + { + return this->operator() ( index ); + } + + //! Equivalent to double& operator()(int index) + double& operator[] ( int index ) + { + return this->operator() ( index ); + } + + inline double x() const; + inline double y() const; + inline double z() const; + inline void x(double); + inline void y(double); + inline void z(double); + + //! Reverses the sign of the Vector object itself + inline void ReverseSign(); + + + //! subtracts a vector from the Vector object itself + inline Vector& operator-=(const Vector& arg); + + + //! Adds a vector from the Vector object itself + inline Vector& operator +=(const Vector& arg); + + //! Scalar multiplication is defined + inline friend Vector operator*(const Vector& lhs,double rhs); + //! Scalar multiplication is defined + inline friend Vector operator*(double lhs,const Vector& rhs); + //! Scalar division is defined + + inline friend Vector operator/(const Vector& lhs,double rhs); + inline friend Vector operator+(const Vector& lhs,const Vector& rhs); + inline friend Vector operator-(const Vector& lhs,const Vector& rhs); + inline friend Vector operator*(const Vector& lhs,const Vector& rhs); + inline friend Vector operator-(const Vector& arg); + inline friend double dot(const Vector& lhs,const Vector& rhs); + + //! To have a uniform operator to put an element to zero, for scalar values + //! and for objects. + inline friend void SetToZero(Vector& v); + + //! @return a zero vector + inline static Vector Zero(); + + /** Normalizes this vector and returns it norm + * makes v a unitvector and returns the norm of v. + * if v is smaller than eps, Vector(1,0,0) is returned with norm 0. + * if this is not good, check the return value of this method. + */ + double Normalize(double eps=epsilon); + + //! @return the norm of the vector + double Norm() const; + + + + //! a 3D vector where the 2D vector v is put in the XY plane + inline void Set2DXY(const Vector2& v); + //! a 3D vector where the 2D vector v is put in the YZ plane + inline void Set2DYZ(const Vector2& v); + //! a 3D vector where the 2D vector v is put in the ZX plane + inline void Set2DZX(const Vector2& v); + //! a 3D vector where the 2D vector v_XY is put in the XY plane of the frame F_someframe_XY. + inline void Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY); + + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const Vector& a,const Vector& b,double eps=epsilon); + + //! return a normalized vector + inline friend Vector Normalize(const Vector& a, double eps=epsilon); + + //! The literal equality operator==(), also identical. + inline friend bool operator==(const Vector& a,const Vector& b); + //! The literal inequality operator!=(). + inline friend bool operator!=(const Vector& a,const Vector& b); + + friend class Rotation; + friend class Frame; +}; + + +/** + \brief represents rotations in 3 dimensional space. + + This class represents a rotation matrix with the following + conventions : + \verbatim + Suppose V2 = R*V, (1) + V is expressed in frame B + V2 is expressed in frame A + This matrix R consists of 3 collumns [ X,Y,Z ], + X,Y, and Z contain the axes of frame B, expressed in frame A + Because of linearity expr(1) is valid. + \endverbatim + This class only represents rotational_interpolation, not translation + Two interpretations are possible for rotation angles. + * if you rotate with angle around X frame A to have frame B, + then the result of SetRotX is equal to frame B expressed wrt A. + In code: + \verbatim + Rotation R; + F_A_B = R.SetRotX(angle); + \endverbatim + * Secondly, if you take the following code : + \verbatim + Vector p,p2; Rotation R; + R.SetRotX(angle); + p2 = R*p; + \endverbatim + then the frame p2 is rotated around X axis with (-angle). + Analogue reasonings can be applyd to SetRotY,SetRotZ,SetRot + \par type + Concrete implementation +*/ +class Rotation +{ +public: + double data[9]; + + inline Rotation() { + *this = Rotation::Identity(); + } + inline Rotation(double Xx,double Yx,double Zx, + double Xy,double Yy,double Zy, + double Xz,double Yz,double Zz); + inline Rotation(const Vector& x,const Vector& y,const Vector& z); + // default copy constructor is sufficient + + inline void setValue(float* oglmat); + inline void getValue(float* oglmat) const; + + inline Rotation& operator=(const Rotation& arg); + + //! Defines a multiplication R*V between a Rotation R and a Vector V. + //! Complexity : 9M+6A + inline Vector operator*(const Vector& v) const; + + //! Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set + inline double& operator()(int i,int j); + + //! Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set + inline double operator() (int i,int j) const; + + friend Rotation operator *(const Rotation& lhs,const Rotation& rhs); + + //! Sets the value of *this to its inverse. + inline void SetInverse(); + + //! Gives back the inverse rotation matrix of *this. + inline Rotation Inverse() const; + + //! The same as R.Inverse()*v but more efficient. + inline Vector Inverse(const Vector& v) const; + + //! The same as R.Inverse()*arg but more efficient. + inline Wrench Inverse(const Wrench& arg) const; + + //! The same as R.Inverse()*arg but more efficient. + inline Twist Inverse(const Twist& arg) const; + + //! Gives back an identity rotaton matrix + inline static Rotation Identity(); + + +// = Rotations + //! The Rot... static functions give the value of the appropriate rotation matrix back. + inline static Rotation RotX(double angle); + //! The Rot... static functions give the value of the appropriate rotation matrix back. + inline static Rotation RotY(double angle); + //! The Rot... static functions give the value of the appropriate rotation matrix back. + inline static Rotation RotZ(double angle); + //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.. + //! DoRot... functions are only defined when they can be executed more efficiently + inline void DoRotX(double angle); + //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.. + //! DoRot... functions are only defined when they can be executed more efficiently + inline void DoRotY(double angle); + //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.. + //! DoRot... functions are only defined when they can be executed more efficiently + inline void DoRotZ(double angle); + + //! Along an arbitrary axes. It is not necessary to normalize rotaxis. + //! returns identity rotation matrix in the case that the norm of rotaxis + //! is to small to be used. + // @see Rot2 if you want to handle this error in another way. + static Rotation Rot(const Vector& rotaxis,double angle); + + //! Along an arbitrary axes. rotvec should be normalized. + static Rotation Rot2(const Vector& rotvec,double angle); + + // make sure the matrix is a pure rotation (no scaling) + void Ortho(); + + //! Returns a vector with the direction of the equiv. axis + //! and its norm is angle + Vector GetRot() const; + + //! Returns a 2D vector representing the equivalent rotation in the XZ plane that brings the + //! Y axis onto the Matrix Y axis and its norm is angle + Vector2 GetXZRot() const; + + /** Returns the rotation angle around the equiv. axis + * @param axis the rotation axis is returned in this variable + * @param eps : in the case of angle == 0 : rot axis is undefined and choosen + * to be +/- Z-axis + * in the case of angle == PI : 2 solutions, positive Z-component + * of the axis is choosen. + * @result returns the rotation angle (between [0..PI] ) + */ + double GetRotAngle(Vector& axis,double eps=epsilon) const; + + + //! Gives back a rotation matrix specified with EulerZYZ convention : + //! First rotate around Z with alfa, + //! then around the new Y with beta, then around + //! new Z with gamma. + static Rotation EulerZYZ(double Alfa,double Beta,double Gamma); + + //! Gives back the EulerZYZ convention description of the rotation matrix : + //! First rotate around Z with alfa, + //! then around the new Y with beta, then around + //! new Z with gamma. + //! + //! Variables are bound by + //! (-PI <= alfa <= PI), + //! (0 <= beta <= PI), + //! (-PI <= alfa <= PI) + void GetEulerZYZ(double& alfa,double& beta,double& gamma) const; + + + //! Sets the value of this object to a rotation specified with RPY convention: + //! first rotate around X with roll, then around the + //! old Y with pitch, then around old Z with alfa + static Rotation RPY(double roll,double pitch,double yaw); + + //! Gives back a vector in RPY coordinates, variables are bound by + //! -PI <= roll <= PI + //! -PI <= Yaw <= PI + //! -PI/2 <= PITCH <= PI/2 + //! + //! convention : first rotate around X with roll, then around the + //! old Y with pitch, then around old Z with alfa + void GetRPY(double& roll,double& pitch,double& yaw) const; + + + //! Gives back a rotation matrix specified with EulerZYX convention : + //! First rotate around Z with alfa, + //! then around the new Y with beta, then around + //! new X with gamma. + //! + //! closely related to RPY-convention + inline static Rotation EulerZYX(double Alfa,double Beta,double Gamma) { + return RPY(Gamma,Beta,Alfa); + } + + //! GetEulerZYX gets the euler ZYX parameters of a rotation : + //! First rotate around Z with alfa, + //! then around the new Y with beta, then around + //! new X with gamma. + //! + //! Range of the results of GetEulerZYX : + //! -PI <= alfa <= PI + //! -PI <= gamma <= PI + //! -PI/2 <= beta <= PI/2 + //! + //! Closely related to RPY-convention. + inline void GetEulerZYX(double& Alfa,double& Beta,double& Gamma) const { + GetRPY(Gamma,Beta,Alfa); + } + + //! Transformation of the base to which the twist is expressed. + //! Complexity : 18M+12A + //! @see Frame*Twist for a transformation that also transforms + //! the velocity reference point. + inline Twist operator * (const Twist& arg) const; + + //! Transformation of the base to which the wrench is expressed. + //! Complexity : 18M+12A + //! @see Frame*Wrench for a transformation that also transforms + //! the force reference point. + inline Wrench operator * (const Wrench& arg) const; + + //! Access to the underlying unitvectors of the rotation matrix + inline Vector UnitX() const { + return Vector(data[0],data[3],data[6]); + } + + //! Access to the underlying unitvectors of the rotation matrix + inline void UnitX(const Vector& X) { + data[0] = X(0); + data[3] = X(1); + data[6] = X(2); + } + + //! Access to the underlying unitvectors of the rotation matrix + inline Vector UnitY() const { + return Vector(data[1],data[4],data[7]); + } + + //! Access to the underlying unitvectors of the rotation matrix + inline void UnitY(const Vector& X) { + data[1] = X(0); + data[4] = X(1); + data[7] = X(2); + } + + //! Access to the underlying unitvectors of the rotation matrix + inline Vector UnitZ() const { + return Vector(data[2],data[5],data[8]); + } + + //! Access to the underlying unitvectors of the rotation matrix + inline void UnitZ(const Vector& X) { + data[2] = X(0); + data[5] = X(1); + data[8] = X(2); + } + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + friend bool Equal(const Rotation& a,const Rotation& b,double eps=epsilon); + + //! The literal equality operator==(), also identical. + friend bool operator==(const Rotation& a,const Rotation& b); + //! The literal inequality operator!=() + friend bool operator!=(const Rotation& a,const Rotation& b); + + friend class Frame; +}; + bool operator==(const Rotation& a,const Rotation& b); + + + +/** + \brief represents a frame transformation in 3D space (rotation + translation) + + if V2 = Frame*V1 (V2 expressed in frame A, V1 expressed in frame B) + then V2 = Frame.M*V1+Frame.p + + Frame.M contains columns that represent the axes of frame B wrt frame A + Frame.p contains the origin of frame B expressed in frame A. +*/ +class Frame { +public: + Vector p; //!< origine of the Frame + Rotation M; //!< Orientation of the Frame + +public: + + inline Frame(const Rotation& R,const Vector& V); + + //! The rotation matrix defaults to identity + explicit inline Frame(const Vector& V); + //! The position matrix defaults to zero + explicit inline Frame(const Rotation& R); + + inline void setValue(float* oglmat); + inline void getValue(float* oglmat) const; + + inline Frame() {} + //! The copy constructor. Normal copy by value semantics. + inline Frame(const Frame& arg); + + //! Reads data from an double array + //\TODO should be formulated as a constructor + void Make4x4(double* d); + + //! Treats a frame as a 4x4 matrix and returns element i,j + //! Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set + inline double operator()(int i,int j); + + //! Treats a frame as a 4x4 matrix and returns element i,j + //! Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set + inline double operator() (int i,int j) const; + + // = Inverse + //! Gives back inverse transformation of a Frame + inline Frame Inverse() const; + + //! The same as p2=R.Inverse()*p but more efficient. + inline Vector Inverse(const Vector& arg) const; + + //! The same as p2=R.Inverse()*p but more efficient. + inline Wrench Inverse(const Wrench& arg) const; + + //! The same as p2=R.Inverse()*p but more efficient. + inline Twist Inverse(const Twist& arg) const; + + //! Normal copy-by-value semantics. + inline Frame& operator = (const Frame& arg); + + //! Transformation of the base to which the vector + //! is expressed. + inline Vector operator * (const Vector& arg) const; + + //! Transformation of both the force reference point + //! and of the base to which the wrench is expressed. + //! look at Rotation*Wrench operator for a transformation + //! of only the base to which the twist is expressed. + //! + //! Complexity : 24M+18A + inline Wrench operator * (const Wrench& arg) const; + + //! Transformation of both the velocity reference point + //! and of the base to which the twist is expressed. + //! look at Rotation*Twist for a transformation of only the + //! base to which the twist is expressed. + //! + //! Complexity : 24M+18A + inline Twist operator * (const Twist& arg) const; + + //! Composition of two frames. + inline friend Frame operator *(const Frame& lhs,const Frame& rhs); + + //! @return the identity transformation Frame(Rotation::Identity(),Vector::Zero()). + inline static Frame Identity(); + + //! The twist <t_this> is expressed wrt the current + //! frame. This frame is integrated into an updated frame with + //! <samplefrequency>. Very simple first order integration rule. + inline void Integrate(const Twist& t_this,double frequency); + + /* + // DH_Craig1989 : constructs a transformationmatrix + // T_link(i-1)_link(i) with the Denavit-Hartenberg convention as + // described in the Craigs book: Craig, J. J.,Introduction to + // Robotics: Mechanics and Control, Addison-Wesley, + // isbn:0-201-10326-5, 1986. + // + // Note that the frame is a redundant way to express the information + // in the DH-convention. + // \verbatim + // Parameters in full : a(i-1),alpha(i-1),d(i),theta(i) + // + // axis i-1 is connected by link i-1 to axis i numbering axis 1 + // to axis n link 0 (immobile base) to link n + // + // link length a(i-1) length of the mutual perpendicular line + // (normal) between the 2 axes. This normal runs from (i-1) to + // (i) axis. + // + // link twist alpha(i-1): construct plane perpendicular to the + // normal project axis(i-1) and axis(i) into plane angle from + // (i-1) to (i) measured in the direction of the normal + // + // link offset d(i) signed distance between normal (i-1) to (i) + // and normal (i) to (i+1) along axis i joint angle theta(i) + // signed angle between normal (i-1) to (i) and normal (i) to + // (i+1) along axis i + // + // First and last joints : a(0)= a(n) = 0 + // alpha(0) = alpha(n) = 0 + // + // PRISMATIC : theta(1) = 0 d(1) arbitrarily + // + // REVOLUTE : theta(1) arbitrarily d(1) = 0 + // + // Not unique : if intersecting joint axis 2 choices for normal + // Frame assignment of the DH convention : Z(i-1) follows axis + // (i-1) X(i-1) is the normal between axis(i-1) and axis(i) + // Y(i-1) follows out of Z(i-1) and X(i-1) + // + // a(i-1) = distance from Z(i-1) to Z(i) along X(i-1) + // alpha(i-1) = angle between Z(i-1) to Z(i) along X(i-1) + // d(i) = distance from X(i-1) to X(i) along Z(i) + // theta(i) = angle between X(i-1) to X(i) along X(i) + // \endverbatim + */ + static Frame DH_Craig1989(double a,double alpha,double d,double theta); + + // DH : constructs a transformationmatrix T_link(i-1)_link(i) with + // the Denavit-Hartenberg convention as described in the original + // publictation: Denavit, J. and Hartenberg, R. S., A kinematic + // notation for lower-pair mechanisms based on matrices, ASME + // Journal of Applied Mechanics, 23:215-221, 1955. + + static Frame DH(double a,double alpha,double d,double theta); + + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const Frame& a,const Frame& b,double eps=epsilon); + + //! The literal equality operator==(), also identical. + inline friend bool operator==(const Frame& a,const Frame& b); + //! The literal inequality operator!=(). + inline friend bool operator!=(const Frame& a,const Frame& b); +}; + +/** + * \brief represents both translational and rotational velocities. + * + * This class represents a twist. A twist is the combination of translational + * velocity and rotational velocity applied at one point. +*/ +class Twist { +public: + Vector vel; //!< The velocity of that point + Vector rot; //!< The rotational velocity of that point. +public: + + //! The default constructor initialises to Zero via the constructor of Vector. + Twist():vel(),rot() {}; + + Twist(const Vector& _vel,const Vector& _rot):vel(_vel),rot(_rot) {}; + + inline Twist& operator-=(const Twist& arg); + inline Twist& operator+=(const Twist& arg); + //! index-based access to components, first vel(0..2), then rot(3..5) + inline double& operator()(int i); + + //! index-based access to components, first vel(0..2), then rot(3..5) + //! For use with a const Twist + inline double operator()(int i) const; + + double operator[] ( int index ) const + { + return this->operator() ( index ); + } + + double& operator[] ( int index ) + { + return this->operator() ( index ); + } + + inline friend Twist operator*(const Twist& lhs,double rhs); + inline friend Twist operator*(double lhs,const Twist& rhs); + inline friend Twist operator/(const Twist& lhs,double rhs); + inline friend Twist operator+(const Twist& lhs,const Twist& rhs); + inline friend Twist operator-(const Twist& lhs,const Twist& rhs); + inline friend Twist operator-(const Twist& arg); + inline friend double dot(const Twist& lhs,const Wrench& rhs); + inline friend double dot(const Wrench& rhs,const Twist& lhs); + inline friend void SetToZero(Twist& v); + + + //! @return a zero Twist : Twist(Vector::Zero(),Vector::Zero()) + static inline Twist Zero(); + + //! Reverses the sign of the twist + inline void ReverseSign(); + + //! Changes the reference point of the twist. + //! The vector v_base_AB is expressed in the same base as the twist + //! The vector v_base_AB is a vector from the old point to + //! the new point. + //! + //! Complexity : 6M+6A + inline Twist RefPoint(const Vector& v_base_AB) const; + + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const Twist& a,const Twist& b,double eps=epsilon); + + //! The literal equality operator==(), also identical. + inline friend bool operator==(const Twist& a,const Twist& b); + //! The literal inequality operator!=(). + inline friend bool operator!=(const Twist& a,const Twist& b); + +// = Friends + friend class Rotation; + friend class Frame; + +}; + +/** + * \brief represents both translational and rotational acceleration. + * + * This class represents an acceleration twist. A acceleration twist is + * the combination of translational + * acceleration and rotational acceleration applied at one point. +*/ +/* +class AccelerationTwist { +public: + Vector trans; //!< The translational acceleration of that point + Vector rot; //!< The rotational acceleration of that point. +public: + + //! The default constructor initialises to Zero via the constructor of Vector. + AccelerationTwist():trans(),rot() {}; + + AccelerationTwist(const Vector& _trans,const Vector& _rot):trans(_trans),rot(_rot) {}; + + inline AccelerationTwist& operator-=(const AccelerationTwist& arg); + inline AccelerationTwist& operator+=(const AccelerationTwist& arg); + //! index-based access to components, first vel(0..2), then rot(3..5) + inline double& operator()(int i); + + //! index-based access to components, first vel(0..2), then rot(3..5) + //! For use with a const AccelerationTwist + inline double operator()(int i) const; + + double operator[] ( int index ) const + { + return this->operator() ( index ); + } + + double& operator[] ( int index ) + { + return this->operator() ( index ); + } + + inline friend AccelerationTwist operator*(const AccelerationTwist& lhs,double rhs); + inline friend AccelerationTwist operator*(double lhs,const AccelerationTwist& rhs); + inline friend AccelerationTwist operator/(const AccelerationTwist& lhs,double rhs); + inline friend AccelerationTwist operator+(const AccelerationTwist& lhs,const AccelerationTwist& rhs); + inline friend AccelerationTwist operator-(const AccelerationTwist& lhs,const AccelerationTwist& rhs); + inline friend AccelerationTwist operator-(const AccelerationTwist& arg); + //inline friend double dot(const AccelerationTwist& lhs,const Wrench& rhs); + //inline friend double dot(const Wrench& rhs,const AccelerationTwist& lhs); + inline friend void SetToZero(AccelerationTwist& v); + + + //! @return a zero AccelerationTwist : AccelerationTwist(Vector::Zero(),Vector::Zero()) + static inline AccelerationTwist Zero(); + + //! Reverses the sign of the AccelerationTwist + inline void ReverseSign(); + + //! Changes the reference point of the AccelerationTwist. + //! The vector v_base_AB is expressed in the same base as the AccelerationTwist + //! The vector v_base_AB is a vector from the old point to + //! the new point. + //! + //! Complexity : 6M+6A + inline AccelerationTwist RefPoint(const Vector& v_base_AB) const; + + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const AccelerationTwist& a,const AccelerationTwist& b,double eps=epsilon); + + //! The literal equality operator==(), also identical. + inline friend bool operator==(const AccelerationTwist& a,const AccelerationTwist& b); + //! The literal inequality operator!=(). + inline friend bool operator!=(const AccelerationTwist& a,const AccelerationTwist& b); + +// = Friends + friend class Rotation; + friend class Frame; + +}; +*/ +/** + * \brief represents the combination of a force and a torque. + * + * This class represents a Wrench. A Wrench is the force and torque applied at a point + */ +class Wrench +{ +public: + Vector force; //!< Force that is applied at the origin of the current ref frame + Vector torque; //!< Torque that is applied at the origin of the current ref frame +public: + + //! Does initialise force and torque to zero via the underlying constructor of Vector + Wrench():force(),torque() {}; + Wrench(const Vector& _force,const Vector& _torque):force(_force),torque(_torque) {}; + +// = Operators + inline Wrench& operator-=(const Wrench& arg); + inline Wrench& operator+=(const Wrench& arg); + + //! index-based access to components, first force(0..2), then torque(3..5) + inline double& operator()(int i); + + //! index-based access to components, first force(0..2), then torque(3..5) + //! for use with a const Wrench + inline double operator()(int i) const; + + double operator[] ( int index ) const + { + return this->operator() ( index ); + } + + double& operator[] ( int index ) + { + return this->operator() ( index ); + } + + //! Scalar multiplication + inline friend Wrench operator*(const Wrench& lhs,double rhs); + //! Scalar multiplication + inline friend Wrench operator*(double lhs,const Wrench& rhs); + //! Scalar division + inline friend Wrench operator/(const Wrench& lhs,double rhs); + + inline friend Wrench operator+(const Wrench& lhs,const Wrench& rhs); + inline friend Wrench operator-(const Wrench& lhs,const Wrench& rhs); + + //! An unary - operator + inline friend Wrench operator-(const Wrench& arg); + + //! Sets the Wrench to Zero, to have a uniform function that sets an object or + //! double to zero. + inline friend void SetToZero(Wrench& v); + + //! @return a zero Wrench + static inline Wrench Zero(); + + //! Reverses the sign of the current Wrench + inline void ReverseSign(); + + //! Changes the reference point of the wrench. + //! The vector v_base_AB is expressed in the same base as the twist + //! The vector v_base_AB is a vector from the old point to + //! the new point. + //! + //! Complexity : 6M+6A + inline Wrench RefPoint(const Vector& v_base_AB) const; + + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const Wrench& a,const Wrench& b,double eps=epsilon); + + //! The literal equality operator==(), also identical. + inline friend bool operator==(const Wrench& a,const Wrench& b); + //! The literal inequality operator!=(). + inline friend bool operator!=(const Wrench& a,const Wrench& b); + + friend class Rotation; + friend class Frame; + + +}; + + +//! 2D version of Vector +class Vector2 +{ + double data[2]; +public: + //! Does not initialise to Zero(). + Vector2() {data[0]=data[1] = 0.0;} + inline Vector2(double x,double y); + inline Vector2(const Vector2& arg); + inline Vector2(double* xyz); + inline Vector2(float* xyz); + + inline Vector2& operator = ( const Vector2& arg); + + //! Access to elements, range checked when NDEBUG is not set, from 0..1 + inline double operator()(int index) const; + + //! Access to elements, range checked when NDEBUG is not set, from 0..1 + inline double& operator() (int index); + + //! store vector components in array + inline void GetValue(double* xy) const; + + inline void ReverseSign(); + inline Vector2& operator-=(const Vector2& arg); + inline Vector2& operator +=(const Vector2& arg); + + + inline friend Vector2 operator*(const Vector2& lhs,double rhs); + inline friend Vector2 operator*(double lhs,const Vector2& rhs); + inline friend Vector2 operator/(const Vector2& lhs,double rhs); + inline friend Vector2 operator+(const Vector2& lhs,const Vector2& rhs); + inline friend Vector2 operator-(const Vector2& lhs,const Vector2& rhs); + inline friend Vector2 operator*(const Vector2& lhs,const Vector2& rhs); + inline friend Vector2 operator-(const Vector2& arg); + inline friend void SetToZero(Vector2& v); + + //! @return a zero 2D vector. + inline static Vector2 Zero(); + + /** Normalizes this vector and returns it norm + * makes v a unitvector and returns the norm of v. + * if v is smaller than eps, Vector(1,0,0) is returned with norm 0. + * if this is not good, check the return value of this method. + */ + double Normalize(double eps=epsilon); + + //! @return the norm of the vector + inline double Norm() const; + + //! projects v in its XY plane, and sets *this to these values + inline void Set3DXY(const Vector& v); + + //! projects v in its YZ plane, and sets *this to these values + inline void Set3DYZ(const Vector& v); + + //! projects v in its ZX plane, and sets *this to these values + inline void Set3DZX(const Vector& v); + + //! projects v_someframe in the XY plane of F_someframe_XY, + //! and sets *this to these values + //! expressed wrt someframe. + inline void Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe); + + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const Vector2& a,const Vector2& b,double eps=epsilon); + + friend class Rotation2; +}; + + +//! A 2D Rotation class, for conventions see Rotation. For further documentation +//! of the methods see Rotation class. +class Rotation2 +{ + double s,c; + //! c,s represent cos(angle), sin(angle), this also represents first col. of rot matrix + //! from outside, this class behaves as if it would store the complete 2x2 matrix. +public: + //! Default constructor does NOT initialise to Zero(). + Rotation2() {c=1.0;s=0.0;} + + explicit Rotation2(double angle_rad):s(sin(angle_rad)),c(cos(angle_rad)) {} + + Rotation2(double ca,double sa):s(sa),c(ca){} + + inline Rotation2& operator=(const Rotation2& arg); + inline Vector2 operator*(const Vector2& v) const; + //! Access to elements 0..1,0..1, bounds are checked when NDEBUG is not set + inline double operator() (int i,int j) const; + + inline friend Rotation2 operator *(const Rotation2& lhs,const Rotation2& rhs); + + inline void SetInverse(); + inline Rotation2 Inverse() const; + inline Vector2 Inverse(const Vector2& v) const; + + inline void SetIdentity(); + inline static Rotation2 Identity(); + + + //! The SetRot.. functions set the value of *this to the appropriate rotation matrix. + inline void SetRot(double angle); + + //! The Rot... static functions give the value of the appropriate rotation matrix bac + inline static Rotation2 Rot(double angle); + + //! Gets the angle (in radians) + inline double GetRot() const; + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const Rotation2& a,const Rotation2& b,double eps=epsilon); +}; + +//! A 2D frame class, for further documentation see the Frames class +//! for methods with unchanged semantics. +class Frame2 + { +public: + Vector2 p; //!< origine of the Frame + Rotation2 M; //!< Orientation of the Frame + +public: + + inline Frame2(const Rotation2& R,const Vector2& V); + explicit inline Frame2(const Vector2& V); + explicit inline Frame2(const Rotation2& R); + inline Frame2(void); + inline Frame2(const Frame2& arg); + inline void Make4x4(double* d); + + //! Treats a frame as a 3x3 matrix and returns element i,j + //! Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set + inline double operator()(int i,int j); + + //! Treats a frame as a 4x4 matrix and returns element i,j + //! Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set + inline double operator() (int i,int j) const; + + inline void SetInverse(); + inline Frame2 Inverse() const; + inline Vector2 Inverse(const Vector2& arg) const; + inline Frame2& operator = (const Frame2& arg); + inline Vector2 operator * (const Vector2& arg); + inline friend Frame2 operator *(const Frame2& lhs,const Frame2& rhs); + inline void SetIdentity(); + inline void Integrate(const Twist& t_this,double frequency); + inline static Frame2 Identity() { + Frame2 tmp; + tmp.SetIdentity(); + return tmp; + } + inline friend bool Equal(const Frame2& a,const Frame2& b,double eps=epsilon); +}; + +IMETHOD Vector diff(const Vector& a,const Vector& b,double dt=1); +IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt=1); +IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt=1); +IMETHOD Twist diff(const Twist& a,const Twist& b,double dt=1); +IMETHOD Wrench diff(const Wrench& W_a_p1,const Wrench& W_a_p2,double dt=1); +IMETHOD Vector addDelta(const Vector& a,const Vector&da,double dt=1); +IMETHOD Rotation addDelta(const Rotation& a,const Vector&da,double dt=1); +IMETHOD Frame addDelta(const Frame& a,const Twist& da,double dt=1); +IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1); +IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1); +#ifdef KDL_INLINE +// #include "vector.inl" +// #include "wrench.inl" + //#include "rotation.inl" + //#include "frame.inl" + //#include "twist.inl" + //#include "vector2.inl" + //#include "rotation2.inl" + //#include "frame2.inl" +#include "frames.inl" +#endif + + + +} + + +#endif diff --git a/intern/itasc/kdl/frames.inl b/intern/itasc/kdl/frames.inl new file mode 100644 index 00000000000..9a176070171 --- /dev/null +++ b/intern/itasc/kdl/frames.inl @@ -0,0 +1,1390 @@ +/*************************************************************************** + frames.inl - description + ------------------------- + begin : June 2006 + copyright : (C) 2006 Erwin Aertbelien + email : firstname.lastname@mech.kuleuven.ac.be + + History (only major changes)( AUTHOR-Description ) : + + *************************************************************************** + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Lesser General Public * + * License as published by the Free Software Foundation; either * + * version 2.1 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with this library; if not, write to the Free Software * + * Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307 USA * + * * + ***************************************************************************/ + + +IMETHOD Vector::Vector(const Vector & arg) +{ + data[0] = arg.data[0]; + data[1] = arg.data[1]; + data[2] = arg.data[2]; +} + +IMETHOD Vector::Vector(double x,double y, double z) +{ + data[0]=x;data[1]=y;data[2]=z; +} + +IMETHOD Vector::Vector(double* xyz) +{ + data[0]=xyz[0];data[1]=xyz[1];data[2]=xyz[2]; +} + +IMETHOD Vector::Vector(float* xyz) +{ + data[0]=xyz[0];data[1]=xyz[1];data[2]=xyz[2]; +} + +IMETHOD void Vector::GetValue(double* xyz) const +{ + xyz[0]=data[0];xyz[1]=data[1];xyz[2]=data[2]; +} + + +IMETHOD Vector& Vector::operator =(const Vector & arg) +{ + data[0] = arg.data[0]; + data[1] = arg.data[1]; + data[2] = arg.data[2]; + return *this; +} + +IMETHOD Vector operator +(const Vector & lhs,const Vector& rhs) +{ + Vector tmp; + tmp.data[0] = lhs.data[0]+rhs.data[0]; + tmp.data[1] = lhs.data[1]+rhs.data[1]; + tmp.data[2] = lhs.data[2]+rhs.data[2]; + return tmp; +} + +IMETHOD Vector operator -(const Vector & lhs,const Vector& rhs) +{ + Vector tmp; + tmp.data[0] = lhs.data[0]-rhs.data[0]; + tmp.data[1] = lhs.data[1]-rhs.data[1]; + tmp.data[2] = lhs.data[2]-rhs.data[2]; + return tmp; +} + +IMETHOD double Vector::x() const { return data[0]; } +IMETHOD double Vector::y() const { return data[1]; } +IMETHOD double Vector::z() const { return data[2]; } + +IMETHOD void Vector::x( double _x ) { data[0] = _x; } +IMETHOD void Vector::y( double _y ) { data[1] = _y; } +IMETHOD void Vector::z( double _z ) { data[2] = _z; } + +Vector operator *(const Vector& lhs,double rhs) +{ + Vector tmp; + tmp.data[0] = lhs.data[0]*rhs; + tmp.data[1] = lhs.data[1]*rhs; + tmp.data[2] = lhs.data[2]*rhs; + return tmp; +} + +Vector operator *(double lhs,const Vector& rhs) +{ + Vector tmp; + tmp.data[0] = lhs*rhs.data[0]; + tmp.data[1] = lhs*rhs.data[1]; + tmp.data[2] = lhs*rhs.data[2]; + return tmp; +} + +Vector operator /(const Vector& lhs,double rhs) +{ + Vector tmp; + tmp.data[0] = lhs.data[0]/rhs; + tmp.data[1] = lhs.data[1]/rhs; + tmp.data[2] = lhs.data[2]/rhs; + return tmp; +} + +Vector operator *(const Vector & lhs,const Vector& rhs) +// Complexity : 6M+3A +{ + Vector tmp; + tmp.data[0] = lhs.data[1]*rhs.data[2]-lhs.data[2]*rhs.data[1]; + tmp.data[1] = lhs.data[2]*rhs.data[0]-lhs.data[0]*rhs.data[2]; + tmp.data[2] = lhs.data[0]*rhs.data[1]-lhs.data[1]*rhs.data[0]; + return tmp; +} + +Vector& Vector::operator +=(const Vector & arg) +// Complexity : 3A +{ + data[0]+=arg.data[0]; + data[1]+=arg.data[1]; + data[2]+=arg.data[2]; + return *this; +} + +Vector& Vector::operator -=(const Vector & arg) +// Complexity : 3A +{ + data[0]-=arg.data[0]; + data[1]-=arg.data[1]; + data[2]-=arg.data[2]; + return *this; +} + +Vector Vector::Zero() +{ + return Vector(0,0,0); +} + +double Vector::operator()(int index) const { + FRAMES_CHECKI((0<=index)&&(index<=2)); + return data[index]; +} + +double& Vector::operator () (int index) +{ + FRAMES_CHECKI((0<=index)&&(index<=2)); + return data[index]; +} + +IMETHOD Vector Normalize(const Vector& a, double eps) +{ + double l=a.Norm(); + return (l<eps) ? Vector(0.0,0.0,0.0) : a/l; +} + +Wrench Frame::operator * (const Wrench& arg) const +// Complexity : 24M+18A +{ + Wrench tmp; + tmp.force = M*arg.force; + tmp.torque = M*arg.torque + p*tmp.force; + return tmp; +} + +Wrench Frame::Inverse(const Wrench& arg) const +{ + Wrench tmp; + tmp.force = M.Inverse(arg.force); + tmp.torque = M.Inverse(arg.torque-p*arg.force); + return tmp; +} + + + +Wrench Rotation::Inverse(const Wrench& arg) const +{ + return Wrench(Inverse(arg.force),Inverse(arg.torque)); +} + +Twist Rotation::Inverse(const Twist& arg) const +{ + return Twist(Inverse(arg.vel),Inverse(arg.rot)); +} + +Wrench Wrench::Zero() +{ + return Wrench(Vector::Zero(),Vector::Zero()); +} + + +void Wrench::ReverseSign() +{ + torque.ReverseSign(); + force.ReverseSign(); +} + +Wrench Wrench::RefPoint(const Vector& v_base_AB) const + // Changes the reference point of the Wrench. + // The vector v_base_AB is expressed in the same base as the twist + // The vector v_base_AB is a vector from the old point to + // the new point. +{ + return Wrench(this->force, + this->torque+this->force*v_base_AB + ); +} + + +Wrench& Wrench::operator-=(const Wrench& arg) +{ + torque-=arg.torque; + force -=arg.force; + return *this; +} + +Wrench& Wrench::operator+=(const Wrench& arg) +{ + torque+=arg.torque; + force +=arg.force; + return *this; +} + +double& Wrench::operator()(int i) +{ + // assert((0<=i)&&(i<6)); done by underlying routines + if (i<3) + return force(i); + else + return torque(i-3); +} + +double Wrench::operator()(int i) const +{ + // assert((0<=i)&&(i<6)); done by underlying routines + if (i<3) + return force(i); + else + return torque(i-3); +} + + +Wrench operator*(const Wrench& lhs,double rhs) +{ + return Wrench(lhs.force*rhs,lhs.torque*rhs); +} + +Wrench operator*(double lhs,const Wrench& rhs) +{ + return Wrench(lhs*rhs.force,lhs*rhs.torque); +} + +Wrench operator/(const Wrench& lhs,double rhs) +{ + return Wrench(lhs.force/rhs,lhs.torque/rhs); +} + +// addition of Wrench's +Wrench operator+(const Wrench& lhs,const Wrench& rhs) +{ + return Wrench(lhs.force+rhs.force,lhs.torque+rhs.torque); +} + +Wrench operator-(const Wrench& lhs,const Wrench& rhs) +{ + return Wrench(lhs.force-rhs.force,lhs.torque-rhs.torque); +} + +// unary - +Wrench operator-(const Wrench& arg) +{ + return Wrench(-arg.force,-arg.torque); +} + +Twist Frame::operator * (const Twist& arg) const +// Complexity : 24M+18A +{ + Twist tmp; + tmp.rot = M*arg.rot; + tmp.vel = M*arg.vel+p*tmp.rot; + return tmp; +} +Twist Frame::Inverse(const Twist& arg) const +{ + Twist tmp; + tmp.rot = M.Inverse(arg.rot); + tmp.vel = M.Inverse(arg.vel-p*arg.rot); + return tmp; +} + +Twist Twist::Zero() +{ + return Twist(Vector::Zero(),Vector::Zero()); +} + + +void Twist::ReverseSign() +{ + vel.ReverseSign(); + rot.ReverseSign(); +} + +Twist Twist::RefPoint(const Vector& v_base_AB) const + // Changes the reference point of the twist. + // The vector v_base_AB is expressed in the same base as the twist + // The vector v_base_AB is a vector from the old point to + // the new point. + // Complexity : 6M+6A +{ + return Twist(this->vel+this->rot*v_base_AB,this->rot); +} + +Twist& Twist::operator-=(const Twist& arg) +{ + vel-=arg.vel; + rot -=arg.rot; + return *this; +} + +Twist& Twist::operator+=(const Twist& arg) +{ + vel+=arg.vel; + rot +=arg.rot; + return *this; +} + +double& Twist::operator()(int i) +{ + // assert((0<=i)&&(i<6)); done by underlying routines + if (i<3) + return vel(i); + else + return rot(i-3); +} + +double Twist::operator()(int i) const +{ + // assert((0<=i)&&(i<6)); done by underlying routines + if (i<3) + return vel(i); + else + return rot(i-3); +} + + +Twist operator*(const Twist& lhs,double rhs) +{ + return Twist(lhs.vel*rhs,lhs.rot*rhs); +} + +Twist operator*(double lhs,const Twist& rhs) +{ + return Twist(lhs*rhs.vel,lhs*rhs.rot); +} + +Twist operator/(const Twist& lhs,double rhs) +{ + return Twist(lhs.vel/rhs,lhs.rot/rhs); +} + +// addition of Twist's +Twist operator+(const Twist& lhs,const Twist& rhs) +{ + return Twist(lhs.vel+rhs.vel,lhs.rot+rhs.rot); +} + +Twist operator-(const Twist& lhs,const Twist& rhs) +{ + return Twist(lhs.vel-rhs.vel,lhs.rot-rhs.rot); +} + +// unary - +Twist operator-(const Twist& arg) +{ + return Twist(-arg.vel,-arg.rot); +} + +Frame::Frame(const Rotation & R) +{ + M=R; + p=Vector::Zero(); +} + +Frame::Frame(const Vector & V) +{ + M = Rotation::Identity(); + p = V; +} + +Frame::Frame(const Rotation & R, const Vector & V) +{ + M = R; + p = V; +} + + Frame operator *(const Frame& lhs,const Frame& rhs) +// Complexity : 36M+36A +{ + return Frame(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); +} + +Vector Frame::operator *(const Vector & arg) const +{ + return M*arg+p; +} + +Vector Frame::Inverse(const Vector& arg) const +{ + return M.Inverse(arg-p); +} + +Frame Frame::Inverse() const +{ + return Frame(M.Inverse(),-M.Inverse(p)); +} + + +Frame& Frame::operator =(const Frame & arg) +{ + M = arg.M; + p = arg.p; + return *this; +} + +Frame::Frame(const Frame & arg) : + p(arg.p),M(arg.M) +{} + + +void Vector::ReverseSign() +{ + data[0] = -data[0]; + data[1] = -data[1]; + data[2] = -data[2]; +} + + + +Vector operator-(const Vector & arg) +{ + Vector tmp; + tmp.data[0]=-arg.data[0]; + tmp.data[1]=-arg.data[1]; + tmp.data[2]=-arg.data[2]; + return tmp; +} + +void Vector::Set2DXY(const Vector2& v) +// a 3D vector where the 2D vector v is put in the XY plane +{ + data[0]=v(0); + data[1]=v(1); + data[2]=0; + +} +void Vector::Set2DYZ(const Vector2& v) +// a 3D vector where the 2D vector v is put in the YZ plane +{ + data[1]=v(0); + data[2]=v(1); + data[0]=0; + +} + +void Vector::Set2DZX(const Vector2& v) +// a 3D vector where the 2D vector v is put in the ZX plane +{ + data[2]=v(0); + data[0]=v(1); + data[1]=0; + +} + + + + + +double& Rotation::operator()(int i,int j) { + FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2)); + return data[i*3+j]; +} + +double Rotation::operator()(int i,int j) const { + FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2)); + return data[i*3+j]; +} + +Rotation::Rotation( double Xx,double Yx,double Zx, + double Xy,double Yy,double Zy, + double Xz,double Yz,double Zz) +{ + data[0] = Xx;data[1]=Yx;data[2]=Zx; + data[3] = Xy;data[4]=Yy;data[5]=Zy; + data[6] = Xz;data[7]=Yz;data[8]=Zz; +} + + +Rotation::Rotation(const Vector& x,const Vector& y,const Vector& z) +{ + data[0] = x.data[0];data[3] = x.data[1];data[6] = x.data[2]; + data[1] = y.data[0];data[4] = y.data[1];data[7] = y.data[2]; + data[2] = z.data[0];data[5] = z.data[1];data[8] = z.data[2]; +} + +Rotation& Rotation::operator=(const Rotation& arg) { + int count=9; + while (count--) data[count] = arg.data[count]; + return *this; +} + +Vector Rotation::operator*(const Vector& v) const { +// Complexity : 9M+6A + return Vector( + data[0]*v.data[0] + data[1]*v.data[1] + data[2]*v.data[2], + data[3]*v.data[0] + data[4]*v.data[1] + data[5]*v.data[2], + data[6]*v.data[0] + data[7]*v.data[1] + data[8]*v.data[2] + ); +} + +Twist Rotation::operator * (const Twist& arg) const + // Transformation of the base to which the twist is expressed. + // look at Frame*Twist for a transformation that also transforms + // the velocity reference point. + // Complexity : 18M+12A +{ + return Twist((*this)*arg.vel,(*this)*arg.rot); +} + +Wrench Rotation::operator * (const Wrench& arg) const + // Transformation of the base to which the wrench is expressed. + // look at Frame*Twist for a transformation that also transforms + // the force reference point. +{ + return Wrench((*this)*arg.force,(*this)*arg.torque); +} + +Rotation Rotation::Identity() { + return Rotation(1,0,0,0,1,0,0,0,1); +} +// *this = *this * ROT(X,angle) +void Rotation::DoRotX(double angle) +{ + double cs = cos(angle); + double sn = sin(angle); + double x1,x2,x3; + x1 = cs* (*this)(0,1) + sn* (*this)(0,2); + x2 = cs* (*this)(1,1) + sn* (*this)(1,2); + x3 = cs* (*this)(2,1) + sn* (*this)(2,2); + (*this)(0,2) = -sn* (*this)(0,1) + cs* (*this)(0,2); + (*this)(1,2) = -sn* (*this)(1,1) + cs* (*this)(1,2); + (*this)(2,2) = -sn* (*this)(2,1) + cs* (*this)(2,2); + (*this)(0,1) = x1; + (*this)(1,1) = x2; + (*this)(2,1) = x3; +} + +void Rotation::DoRotY(double angle) +{ + double cs = cos(angle); + double sn = sin(angle); + double x1,x2,x3; + x1 = cs* (*this)(0,0) - sn* (*this)(0,2); + x2 = cs* (*this)(1,0) - sn* (*this)(1,2); + x3 = cs* (*this)(2,0) - sn* (*this)(2,2); + (*this)(0,2) = sn* (*this)(0,0) + cs* (*this)(0,2); + (*this)(1,2) = sn* (*this)(1,0) + cs* (*this)(1,2); + (*this)(2,2) = sn* (*this)(2,0) + cs* (*this)(2,2); + (*this)(0,0) = x1; + (*this)(1,0) = x2; + (*this)(2,0) = x3; +} + +void Rotation::DoRotZ(double angle) +{ + double cs = cos(angle); + double sn = sin(angle); + double x1,x2,x3; + x1 = cs* (*this)(0,0) + sn* (*this)(0,1); + x2 = cs* (*this)(1,0) + sn* (*this)(1,1); + x3 = cs* (*this)(2,0) + sn* (*this)(2,1); + (*this)(0,1) = -sn* (*this)(0,0) + cs* (*this)(0,1); + (*this)(1,1) = -sn* (*this)(1,0) + cs* (*this)(1,1); + (*this)(2,1) = -sn* (*this)(2,0) + cs* (*this)(2,1); + (*this)(0,0) = x1; + (*this)(1,0) = x2; + (*this)(2,0) = x3; +} + + +Rotation Rotation::RotX(double angle) { + double cs=cos(angle); + double sn=sin(angle); + return Rotation(1,0,0,0,cs,-sn,0,sn,cs); +} +Rotation Rotation::RotY(double angle) { + double cs=cos(angle); + double sn=sin(angle); + return Rotation(cs,0,sn,0,1,0,-sn,0,cs); +} +Rotation Rotation::RotZ(double angle) { + double cs=cos(angle); + double sn=sin(angle); + return Rotation(cs,-sn,0,sn,cs,0,0,0,1); +} + + + + +void Frame::Integrate(const Twist& t_this,double samplefrequency) +{ + double n = t_this.rot.Norm()/samplefrequency; + if (n<epsilon) { + p += M*(t_this.vel/samplefrequency); + } else { + (*this) = (*this) * + Frame ( Rotation::Rot( t_this.rot, n ), + t_this.vel/samplefrequency + ); + } +} + +Rotation Rotation::Inverse() const +{ + Rotation tmp(*this); + tmp.SetInverse(); + return tmp; +} + +Vector Rotation::Inverse(const Vector& v) const { + return Vector( + data[0]*v.data[0] + data[3]*v.data[1] + data[6]*v.data[2], + data[1]*v.data[0] + data[4]*v.data[1] + data[7]*v.data[2], + data[2]*v.data[0] + data[5]*v.data[1] + data[8]*v.data[2] + ); +} + +void Rotation::setValue(float* oglmat) +{ + data[0] = *oglmat++; data[3] = *oglmat++; data[6] = *oglmat++; oglmat++; + data[1] = *oglmat++; data[4] = *oglmat++; data[7] = *oglmat++; oglmat++; + data[2] = *oglmat++; data[5] = *oglmat++; data[8] = *oglmat; + Ortho(); +} + +void Rotation::getValue(float* oglmat) const +{ + *oglmat++ = (float)data[0]; *oglmat++ = (float)data[3]; *oglmat++ = (float)data[6]; *oglmat++ = 0.f; + *oglmat++ = (float)data[1]; *oglmat++ = (float)data[4]; *oglmat++ = (float)data[7]; *oglmat++ = 0.f; + *oglmat++ = (float)data[2]; *oglmat++ = (float)data[5]; *oglmat++ = (float)data[8]; *oglmat++ = 0.f; + *oglmat++ = 0.f; *oglmat++ = 0.f; *oglmat++ = 0.f; *oglmat = 1.f; +} + +void Rotation::SetInverse() +{ + double tmp; + tmp = data[1];data[1]=data[3];data[3]=tmp; + tmp = data[2];data[2]=data[6];data[6]=tmp; + tmp = data[5];data[5]=data[7];data[7]=tmp; +} + + + + + + + +double Frame::operator()(int i,int j) { + FRAMES_CHECKI((0<=i)&&(i<=3)&&(0<=j)&&(j<=3)); + if (i==3) { + if (j==3) + return 1.0; + else + return 0.0; + } else { + if (j==3) + return p(i); + else + return M(i,j); + + } +} + +double Frame::operator()(int i,int j) const { + FRAMES_CHECKI((0<=i)&&(i<=3)&&(0<=j)&&(j<=3)); + if (i==3) { + if (j==3) + return 1; + else + return 0; + } else { + if (j==3) + return p(i); + else + return M(i,j); + + } +} + + +Frame Frame::Identity() { + return Frame(Rotation::Identity(),Vector::Zero()); +} + + +void Frame::setValue(float* oglmat) +{ + M.setValue(oglmat); + p.data[0] = oglmat[12]; + p.data[1] = oglmat[13]; + p.data[2] = oglmat[14]; +} + +void Frame::getValue(float* oglmat) const +{ + M.getValue(oglmat); + oglmat[12] = (float)p.data[0]; + oglmat[13] = (float)p.data[1]; + oglmat[14] = (float)p.data[2]; +} + +void Vector::Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY) +// a 3D vector where the 2D vector v is put in the XY plane of the frame +// F_someframe_XY. +{ +Vector tmp_XY; +tmp_XY.Set2DXY(v_XY); +tmp_XY = F_someframe_XY*(tmp_XY); +} + + + + + + + + + +//============ 2 dimensional version of the frames objects ============= +IMETHOD Vector2::Vector2(const Vector2 & arg) +{ + data[0] = arg.data[0]; + data[1] = arg.data[1]; +} + +IMETHOD Vector2::Vector2(double x,double y) +{ + data[0]=x;data[1]=y; +} + +IMETHOD Vector2::Vector2(double* xy) +{ + data[0]=xy[0];data[1]=xy[1]; +} + +IMETHOD Vector2::Vector2(float* xy) +{ + data[0]=xy[0];data[1]=xy[1]; +} + +IMETHOD Vector2& Vector2::operator =(const Vector2 & arg) +{ + data[0] = arg.data[0]; + data[1] = arg.data[1]; + return *this; +} + +IMETHOD void Vector2::GetValue(double* xy) const +{ + xy[0]=data[0];xy[1]=data[1]; +} + +IMETHOD Vector2 operator +(const Vector2 & lhs,const Vector2& rhs) +{ + return Vector2(lhs.data[0]+rhs.data[0],lhs.data[1]+rhs.data[1]); +} + +IMETHOD Vector2 operator -(const Vector2 & lhs,const Vector2& rhs) +{ + return Vector2(lhs.data[0]-rhs.data[0],lhs.data[1]-rhs.data[1]); +} + +IMETHOD Vector2 operator *(const Vector2& lhs,double rhs) +{ + return Vector2(lhs.data[0]*rhs,lhs.data[1]*rhs); +} + +IMETHOD Vector2 operator *(double lhs,const Vector2& rhs) +{ + return Vector2(lhs*rhs.data[0],lhs*rhs.data[1]); +} + +IMETHOD Vector2 operator /(const Vector2& lhs,double rhs) +{ + return Vector2(lhs.data[0]/rhs,lhs.data[1]/rhs); +} + +IMETHOD Vector2& Vector2::operator +=(const Vector2 & arg) +{ + data[0]+=arg.data[0]; + data[1]+=arg.data[1]; + return *this; +} + +IMETHOD Vector2& Vector2::operator -=(const Vector2 & arg) +{ + data[0]-=arg.data[0]; + data[1]-=arg.data[1]; + return *this; +} + +IMETHOD Vector2 Vector2::Zero() { + return Vector2(0,0); +} + +IMETHOD double Vector2::operator()(int index) const { + FRAMES_CHECKI((0<=index)&&(index<=1)); + return data[index]; +} + +IMETHOD double& Vector2::operator () (int index) +{ + FRAMES_CHECKI((0<=index)&&(index<=1)); + return data[index]; +} +IMETHOD void Vector2::ReverseSign() +{ + data[0] = -data[0]; + data[1] = -data[1]; +} + + +IMETHOD Vector2 operator-(const Vector2 & arg) +{ + return Vector2(-arg.data[0],-arg.data[1]); +} + + +IMETHOD void Vector2::Set3DXY(const Vector& v) +// projects v in its XY plane, and sets *this to these values +{ + data[0]=v(0); + data[1]=v(1); +} +IMETHOD void Vector2::Set3DYZ(const Vector& v) +// projects v in its XY plane, and sets *this to these values +{ + data[0]=v(1); + data[1]=v(2); +} +IMETHOD void Vector2::Set3DZX(const Vector& v) +// projects v in its XY plane, and and sets *this to these values +{ + data[0]=v(2); + data[1]=v(0); +} + +IMETHOD void Vector2::Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe) +// projects v in the XY plane of F_someframe_XY, and sets *this to these values +// expressed wrt someframe. +{ + Vector tmp = F_someframe_XY.Inverse(v_someframe); + data[0]=tmp(0); + data[1]=tmp(1); +} + + + +IMETHOD Rotation2& Rotation2::operator=(const Rotation2& arg) { + c=arg.c;s=arg.s; + return *this; +} + +IMETHOD Vector2 Rotation2::operator*(const Vector2& v) const { + return Vector2(v.data[0]*c-v.data[1]*s,v.data[0]*s+v.data[1]*c); +} + +IMETHOD double Rotation2::operator()(int i,int j) const { + FRAMES_CHECKI((0<=i)&&(i<=1)&&(0<=j)&&(j<=1)); + if (i==j) return c; + if (i==0) + return s; + else + return -s; +} + + +IMETHOD Rotation2 operator *(const Rotation2& lhs,const Rotation2& rhs) { + return Rotation2(lhs.c*rhs.c-lhs.s*rhs.s,lhs.s*rhs.c+lhs.c*rhs.s); +} + +IMETHOD void Rotation2::SetInverse() { + s=-s; +} + +IMETHOD Rotation2 Rotation2::Inverse() const { + return Rotation2(c,-s); +} + +IMETHOD Vector2 Rotation2::Inverse(const Vector2& v) const { + return Vector2(v.data[0]*c+v.data[1]*s,-v.data[0]*s+v.data[1]*c); +} + +IMETHOD Rotation2 Rotation2::Identity() { + return Rotation2(1,0); +} + +IMETHOD void Rotation2::SetIdentity() +{ + c = 1; + s = 0; +} + +IMETHOD void Rotation2::SetRot(double angle) { + c=cos(angle);s=sin(angle); +} + +IMETHOD Rotation2 Rotation2::Rot(double angle) { + return Rotation2(cos(angle),sin(angle)); +} + +IMETHOD double Rotation2::GetRot() const { + return atan2(s,c); +} + + +IMETHOD Frame2::Frame2() { +} + +IMETHOD Frame2::Frame2(const Rotation2 & R) +{ + M=R; + p=Vector2::Zero(); +} + +IMETHOD Frame2::Frame2(const Vector2 & V) +{ + M = Rotation2::Identity(); + p = V; +} + +IMETHOD Frame2::Frame2(const Rotation2 & R, const Vector2 & V) +{ + M = R; + p = V; +} + +IMETHOD Frame2 operator *(const Frame2& lhs,const Frame2& rhs) +{ + return Frame2(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); +} + +IMETHOD Vector2 Frame2::operator *(const Vector2 & arg) +{ + return M*arg+p; +} + +IMETHOD Vector2 Frame2::Inverse(const Vector2& arg) const +{ + return M.Inverse(arg-p); +} + +IMETHOD void Frame2::SetIdentity() +{ + M.SetIdentity(); + p = Vector2::Zero(); +} + +IMETHOD void Frame2::SetInverse() +{ + M.SetInverse(); + p = M*p; + p.ReverseSign(); +} + + +IMETHOD Frame2 Frame2::Inverse() const +{ + Frame2 tmp(*this); + tmp.SetInverse(); + return tmp; +} + +IMETHOD Frame2& Frame2::operator =(const Frame2 & arg) +{ + M = arg.M; + p = arg.p; + return *this; +} + +IMETHOD Frame2::Frame2(const Frame2 & arg) : + p(arg.p), M(arg.M) +{} + + +IMETHOD double Frame2::operator()(int i,int j) { + FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2)); + if (i==2) { + if (j==2) + return 1; + else + return 0; + } else { + if (j==2) + return p(i); + else + return M(i,j); + + } +} + +IMETHOD double Frame2::operator()(int i,int j) const { + FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2)); + if (i==2) { + if (j==2) + return 1; + else + return 0; + } else { + if (j==2) + return p(i); + else + return M(i,j); + + } +} + +// Scalar products. + +IMETHOD double dot(const Vector& lhs,const Vector& rhs) { + return rhs(0)*lhs(0)+rhs(1)*lhs(1)+rhs(2)*lhs(2); +} + +IMETHOD double dot(const Twist& lhs,const Wrench& rhs) { + return dot(lhs.vel,rhs.force)+dot(lhs.rot,rhs.torque); +} + +IMETHOD double dot(const Wrench& rhs,const Twist& lhs) { + return dot(lhs.vel,rhs.force)+dot(lhs.rot,rhs.torque); +} + + + + + +// Equality operators + + + +IMETHOD bool Equal(const Vector& a,const Vector& b,double eps) { + return (Equal(a.data[0],b.data[0],eps)&& + Equal(a.data[1],b.data[1],eps)&& + Equal(a.data[2],b.data[2],eps) ); + } + + +IMETHOD bool Equal(const Frame& a,const Frame& b,double eps) { + return (Equal(a.p,b.p,eps)&& + Equal(a.M,b.M,eps) ); +} + +IMETHOD bool Equal(const Wrench& a,const Wrench& b,double eps) { + return (Equal(a.force,b.force,eps)&& + Equal(a.torque,b.torque,eps) ); +} + +IMETHOD bool Equal(const Twist& a,const Twist& b,double eps) { + return (Equal(a.rot,b.rot,eps)&& + Equal(a.vel,b.vel,eps) ); +} + +IMETHOD bool Equal(const Vector2& a,const Vector2& b,double eps) { + return (Equal(a.data[0],b.data[0],eps)&& + Equal(a.data[1],b.data[1],eps) ); + } + +IMETHOD bool Equal(const Rotation2& a,const Rotation2& b,double eps) { + return ( Equal(a.c,b.c,eps) && Equal(a.s,b.s,eps) ); +} + +IMETHOD bool Equal(const Frame2& a,const Frame2& b,double eps) { + return (Equal(a.p,b.p,eps)&& + Equal(a.M,b.M,eps) ); +} + +IMETHOD void SetToZero(Vector& v) { + v=Vector::Zero(); +} +IMETHOD void SetToZero(Twist& v) { + SetToZero(v.rot); + SetToZero(v.vel); +} +IMETHOD void SetToZero(Wrench& v) { + SetToZero(v.force); + SetToZero(v.torque); +} + +IMETHOD void SetToZero(Vector2& v) { + v = Vector2::Zero(); +} + + +//////////////////////////////////////////////////////////////// +// The following defines the operations +// diff +// addDelta +// random +// posrandom +// on all the types defined in this library. +// (mostly for uniform integration, differentiation and testing). +// Defined as functions because double is not a class and a method +// would brake uniformity when defined for a double. +//////////////////////////////////////////////////////////////// + + + + + + +/** + * axis_a_b is a rotation vector, its norm is a rotation angle + * axis_a_b rotates the a frame towards the b frame. + * This routine returns the rotation matrix R_a_b + */ +IMETHOD Rotation Rot(const Vector& axis_a_b) { + // The formula is + // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr)) + // can be found by multiplying it with an arbitrary vector p + // and noting that this vector is rotated. + Vector rotvec = axis_a_b; + double angle = rotvec.Normalize(1E-10); + double ct = ::cos(angle); + double st = ::sin(angle); + double vt = 1-ct; + return Rotation( + ct + vt*rotvec(0)*rotvec(0), + -rotvec(2)*st + vt*rotvec(0)*rotvec(1), + rotvec(1)*st + vt*rotvec(0)*rotvec(2), + rotvec(2)*st + vt*rotvec(1)*rotvec(0), + ct + vt*rotvec(1)*rotvec(1), + -rotvec(0)*st + vt*rotvec(1)*rotvec(2), + -rotvec(1)*st + vt*rotvec(2)*rotvec(0), + rotvec(0)*st + vt*rotvec(2)*rotvec(1), + ct + vt*rotvec(2)*rotvec(2) + ); + } + +IMETHOD Vector diff(const Vector& a,const Vector& b,double dt) { + return (b-a)/dt; +} + +/** + * \brief diff operator for displacement rotational velocity. + * + * The Vector arguments here represent a displacement rotational velocity. i.e. a rotation + * around a fixed axis for a certain angle. For this representation you cannot use diff() but + * have to use diff_displ(). + * + * \TODO represent a displacement twist and displacement rotational velocity with another + * class, instead of Vector and Twist. + * \warning do not confuse displacement rotational velocities and velocities + * \warning do not confuse displacement twist and twist. + * +IMETHOD Vector diff_displ(const Vector& a,const Vector& b,double dt) { + return diff(Rot(a),Rot(b),dt); +}*/ + +/** + * \brief diff operator for displacement twist. + * + * The Twist arguments here represent a displacement twist. i.e. a rotation + * around a fixed axis for a certain angle. For this representation you cannot use diff() but + * have to use diff_displ(). + * + * \warning do not confuse displacement rotational velocities and velocities + * \warning do not confuse displacement twist and twist. + * + +IMETHOD Twist diff_displ(const Twist& a,const Twist& b,double dt) { + return Twist(diff(a.vel,b.vel,dt),diff(Rot(a.rot),Rot(b.rot),dt)); +} +*/ + +IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt) { + Rotation R_b1_b2(R_a_b1.Inverse()*R_a_b2); + return R_a_b1 * R_b1_b2.GetRot() / dt; +} +IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt) { + return Twist( + diff(F_a_b1.p,F_a_b2.p,dt), + diff(F_a_b1.M,F_a_b2.M,dt) + ); +} +IMETHOD Twist diff(const Twist& a,const Twist& b,double dt) { + return Twist(diff(a.vel,b.vel,dt),diff(a.rot,b.rot,dt)); +} + +IMETHOD Wrench diff(const Wrench& a,const Wrench& b,double dt) { + return Wrench( + diff(a.force,b.force,dt), + diff(a.torque,b.torque,dt) + ); +} + + +IMETHOD Vector addDelta(const Vector& a,const Vector&da,double dt) { + return a+da*dt; +} + +IMETHOD Rotation addDelta(const Rotation& a,const Vector&da,double dt) { + return a*Rot(a.Inverse(da)*dt); +} +IMETHOD Frame addDelta(const Frame& a,const Twist& da,double dt) { + return Frame( + addDelta(a.M,da.rot,dt), + addDelta(a.p,da.vel,dt) + ); +} +IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt) { + return Twist(addDelta(a.vel,da.vel,dt),addDelta(a.rot,da.rot,dt)); +} +IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt) { + return Wrench(addDelta(a.force,da.force,dt),addDelta(a.torque,da.torque,dt)); +} + + +/** + * \brief addDelta operator for displacement rotational velocity. + * + * The Vector arguments here represent a displacement rotational velocity. i.e. a rotation + * around a fixed axis for a certain angle. For this representation you cannot use diff() but + * have to use diff_displ(). + * + * \param a : displacement rotational velocity + * \param da : rotational velocity + * \return displacement rotational velocity + * + * \warning do not confuse displacement rotational velocities and velocities + * \warning do not confuse displacement twist and twist. + * +IMETHOD Vector addDelta_displ(const Vector& a,const Vector&da,double dt) { + return getRot(addDelta(Rot(a),da,dt)); +}*/ + +/** + * \brief addDelta operator for displacement twist. + * + * The Vector arguments here represent a displacement rotational velocity. i.e. a rotation + * around a fixed axis for a certain angle. For this representation you cannot use diff() but + * have to use diff_displ(). + * + * \param a : displacement twist + * \param da : twist + * \return displacement twist + * + * \warning do not confuse displacement rotational velocities and velocities + * \warning do not confuse displacement twist and twist. + * +IMETHOD Twist addDelta_displ(const Twist& a,const Twist&da,double dt) { + return Twist(addDelta(a.vel,da.vel,dt),addDelta_displ(a.rot,da.rot,dt)); +}*/ + + +IMETHOD void random(Vector& a) { + random(a[0]); + random(a[1]); + random(a[2]); +} +IMETHOD void random(Twist& a) { + random(a.rot); + random(a.vel); +} +IMETHOD void random(Wrench& a) { + random(a.torque); + random(a.force); +} + +IMETHOD void random(Rotation& R) { + double alfa; + double beta; + double gamma; + random(alfa); + random(beta); + random(gamma); + R = Rotation::EulerZYX(alfa,beta,gamma); +} + +IMETHOD void random(Frame& F) { + random(F.M); + random(F.p); +} + +IMETHOD void posrandom(Vector& a) { + posrandom(a[0]); + posrandom(a[1]); + posrandom(a[2]); +} +IMETHOD void posrandom(Twist& a) { + posrandom(a.rot); + posrandom(a.vel); +} +IMETHOD void posrandom(Wrench& a) { + posrandom(a.torque); + posrandom(a.force); +} + +IMETHOD void posrandom(Rotation& R) { + double alfa; + double beta; + double gamma; + posrandom(alfa); + posrandom(beta); + posrandom(gamma); + R = Rotation::EulerZYX(alfa,beta,gamma); +} + +IMETHOD void posrandom(Frame& F) { + random(F.M); + random(F.p); +} + + + + +IMETHOD bool operator==(const Frame& a,const Frame& b ) { +#ifdef KDL_USE_EQUAL + return Equal(a,b); +#else + return (a.p == b.p && + a.M == b.M ); +#endif +} + +IMETHOD bool operator!=(const Frame& a,const Frame& b) { + return !operator==(a,b); +} + +IMETHOD bool operator==(const Vector& a,const Vector& b) { +#ifdef KDL_USE_EQUAL + return Equal(a,b); +#else + return (a.data[0]==b.data[0]&& + a.data[1]==b.data[1]&& + a.data[2]==b.data[2] ); +#endif + } + +IMETHOD bool operator!=(const Vector& a,const Vector& b) { + return !operator==(a,b); +} + +IMETHOD bool operator==(const Twist& a,const Twist& b) { +#ifdef KDL_USE_EQUAL + return Equal(a,b); +#else + return (a.rot==b.rot && + a.vel==b.vel ); +#endif +} + +IMETHOD bool operator!=(const Twist& a,const Twist& b) { + return !operator==(a,b); +} + +IMETHOD bool operator==(const Wrench& a,const Wrench& b ) { +#ifdef KDL_USE_EQUAL + return Equal(a,b); +#else + return (a.force==b.force && + a.torque==b.torque ); +#endif +} + +IMETHOD bool operator!=(const Wrench& a,const Wrench& b) { + return !operator==(a,b); +} +IMETHOD bool operator!=(const Rotation& a,const Rotation& b) { + return !operator==(a,b); +} + diff --git a/intern/itasc/kdl/frames_io.cpp b/intern/itasc/kdl/frames_io.cpp new file mode 100644 index 00000000000..0af50bb0e07 --- /dev/null +++ b/intern/itasc/kdl/frames_io.cpp @@ -0,0 +1,310 @@ + +/*************************************************************************** + frames_io.h - description + ------------------------- + begin : June 2006 + copyright : (C) 2006 Erwin Aertbelien + email : firstname.lastname@mech.kuleuven.ac.be + + History (only major changes)( AUTHOR-Description ) : + + *************************************************************************** + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Lesser General Public * + * License as published by the Free Software Foundation; either * + * version 2.1 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with this library; if not, write to the Free Software * + * Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307 USA * + * * + ***************************************************************************/ + +#include "utilities/error.h" +#include "utilities/error_stack.h" +#include "frames.hpp" +#include "frames_io.hpp" + +#include <stdlib.h> +#include <ctype.h> +#include <string.h> +#include <iostream> + +namespace KDL { + + +std::ostream& operator << (std::ostream& os,const Vector& v) { + os << "[" << std::setw(KDL_FRAME_WIDTH) << v(0) << "," << std::setw(KDL_FRAME_WIDTH)<<v(1) + << "," << std::setw(KDL_FRAME_WIDTH) << v(2) << "]"; + return os; +} + +std::ostream& operator << (std::ostream& os,const Twist& v) { + os << "[" << std::setw(KDL_FRAME_WIDTH) << v.vel(0) + << "," << std::setw(KDL_FRAME_WIDTH) << v.vel(1) + << "," << std::setw(KDL_FRAME_WIDTH) << v.vel(2) + << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(0) + << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(1) + << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(2) + << "]"; + return os; +} + +std::ostream& operator << (std::ostream& os,const Wrench& v) { + os << "[" << std::setw(KDL_FRAME_WIDTH) << v.force(0) + << "," << std::setw(KDL_FRAME_WIDTH) << v.force(1) + << "," << std::setw(KDL_FRAME_WIDTH) << v.force(2) + << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(0) + << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(1) + << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(2) + << "]"; + return os; +} + + +std::ostream& operator << (std::ostream& os,const Rotation& R) { +#ifdef KDL_ROTATION_PROPERTIES_RPY + double r,p,y; + R.GetRPY(r,p,y); + os << "[RPY]"<<endl; + os << "["; + os << std::setw(KDL_FRAME_WIDTH) << r << ","; + os << std::setw(KDL_FRAME_WIDTH) << p << ","; + os << std::setw(KDL_FRAME_WIDTH) << y << "]"; +#else +# ifdef KDL_ROTATION_PROPERTIES_EULER + double z,y,x; + R.GetEulerZYX(z,y,x); + os << "[EULERZYX]"<<endl; + os << "["; + os << std::setw(KDL_FRAME_WIDTH) << z << ","; + os << std::setw(KDL_FRAME_WIDTH) << y << ","; + os << std::setw(KDL_FRAME_WIDTH) << x << "]"; +# else + os << "["; + for (int i=0;i<=2;i++) { + os << std::setw(KDL_FRAME_WIDTH) << R(i,0) << "," << + std::setw(KDL_FRAME_WIDTH) << R(i,1) << "," << + std::setw(KDL_FRAME_WIDTH) << R(i,2); + if (i<2) + os << ";"<< std::endl << " "; + else + os << "]"; + } +# endif +#endif + return os; +} + +std::ostream& operator << (std::ostream& os, const Frame& T) +{ + os << "[" << T.M << std::endl<< T.p << "]"; + return os; +} + +std::ostream& operator << (std::ostream& os,const Vector2& v) { + os << "[" << std::setw(KDL_FRAME_WIDTH) << v(0) << "," << std::setw(KDL_FRAME_WIDTH)<<v(1) + << "]"; + return os; +} + +// Rotation2 gives back an angle in degrees with the << and >> operators. +std::ostream& operator << (std::ostream& os,const Rotation2& R) { + os << "[" << R.GetRot()*rad2deg << "]"; + return os; +} + +std::ostream& operator << (std::ostream& os, const Frame2& T) +{ + os << T.M << T.p; + return os; +} + +std::istream& operator >> (std::istream& is,Vector& v) +{ IOTrace("Stream input Vector (vector or ZERO)"); + char storage[10]; + EatWord(is,"[]",storage,10); + if (strlen(storage)==0) { + Eat(is,'['); + is >> v(0); + Eat(is,','); + is >> v(1); + Eat(is,','); + is >> v(2); + EatEnd(is,']'); + IOTracePop(); + return is; + } + if (strcmp(storage,"ZERO")==0) { + v = Vector::Zero(); + IOTracePop(); + return is; + } + throw Error_Frame_Vector_Unexpected_id(); +} + +std::istream& operator >> (std::istream& is,Twist& v) +{ IOTrace("Stream input Twist"); + Eat(is,'['); + is >> v.vel(0); + Eat(is,','); + is >> v.vel(1); + Eat(is,','); + is >> v.vel(2); + Eat(is,','); + is >> v.rot(0); + Eat(is,','); + is >> v.rot(1); + Eat(is,','); + is >> v.rot(2); + EatEnd(is,']'); + IOTracePop(); + return is; +} + +std::istream& operator >> (std::istream& is,Wrench& v) +{ IOTrace("Stream input Wrench"); + Eat(is,'['); + is >> v.force(0); + Eat(is,','); + is >> v.force(1); + Eat(is,','); + is >> v.force(2); + Eat(is,','); + is >> v.torque(0); + Eat(is,','); + is >> v.torque(1); + Eat(is,','); + is >> v.torque(2); + EatEnd(is,']'); + IOTracePop(); + return is; +} + +std::istream& operator >> (std::istream& is,Rotation& r) +{ IOTrace("Stream input Rotation (Matrix or EULERZYX, EULERZYZ,RPY, ROT, IDENTITY)"); + char storage[10]; + EatWord(is,"[]",storage,10); + if (strlen(storage)==0) { + Eat(is,'['); + for (int i=0;i<3;i++) { + is >> r(i,0); + Eat(is,',') ; + is >> r(i,1); + Eat(is,','); + is >> r(i,2); + if (i<2) + Eat(is,';'); + else + EatEnd(is,']'); + } + IOTracePop(); + return is; + } + Vector v; + if (strcmp(storage,"EULERZYX")==0) { + is >> v; + v=v*deg2rad; + r = Rotation::EulerZYX(v(0),v(1),v(2)); + IOTracePop(); + return is; + } + if (strcmp(storage,"EULERZYZ")==0) { + is >> v; + v=v*deg2rad; + r = Rotation::EulerZYZ(v(0),v(1),v(2)); + IOTracePop(); + return is; + } + if (strcmp(storage,"RPY")==0) { + is >> v; + v=v*deg2rad; + r = Rotation::RPY(v(0),v(1),v(2)); + IOTracePop(); + return is; + } + if (strcmp(storage,"ROT")==0) { + is >> v; + double angle; + Eat(is,'['); + is >> angle; + EatEnd(is,']'); + r = Rotation::Rot(v,angle*deg2rad); + IOTracePop(); + return is; + } + if (strcmp(storage,"IDENTITY")==0) { + r = Rotation::Identity(); + IOTracePop(); + return is; + } + throw Error_Frame_Rotation_Unexpected_id(); + return is; +} + +std::istream& operator >> (std::istream& is,Frame& T) +{ IOTrace("Stream input Frame (Rotation,Vector) or DH[...]"); + char storage[10]; + EatWord(is,"[",storage,10); + if (strlen(storage)==0) { + Eat(is,'['); + is >> T.M; + is >> T.p; + EatEnd(is,']'); + IOTracePop(); + return is; + } + if (strcmp(storage,"DH")==0) { + double a,alpha,d,theta; + Eat(is,'['); + is >> a; + Eat(is,','); + is >> alpha; + Eat(is,','); + is >> d; + Eat(is,','); + is >> theta; + EatEnd(is,']'); + T = Frame::DH(a,alpha*deg2rad,d,theta*deg2rad); + IOTracePop(); + return is; + } + throw Error_Frame_Frame_Unexpected_id(); + return is; +} + +std::istream& operator >> (std::istream& is,Vector2& v) +{ IOTrace("Stream input Vector2"); + Eat(is,'['); + is >> v(0); + Eat(is,','); + is >> v(1); + EatEnd(is,']'); + IOTracePop(); + return is; +} +std::istream& operator >> (std::istream& is,Rotation2& r) +{ IOTrace("Stream input Rotation2"); + Eat(is,'['); + double val; + is >> val; + r.Rot(val*deg2rad); + EatEnd(is,']'); + IOTracePop(); + return is; +} +std::istream& operator >> (std::istream& is,Frame2& T) +{ IOTrace("Stream input Frame2"); + is >> T.M; + is >> T.p; + IOTracePop(); + return is; +} + +} // namespace Frame diff --git a/intern/itasc/kdl/frames_io.hpp b/intern/itasc/kdl/frames_io.hpp new file mode 100644 index 00000000000..a358d27383f --- /dev/null +++ b/intern/itasc/kdl/frames_io.hpp @@ -0,0 +1,114 @@ +/*************************************************************************** + frames_io.h - description + ------------------------- + begin : June 2006 + copyright : (C) 2006 Erwin Aertbelien + email : firstname.lastname@mech.kuleuven.ac.be + + History (only major changes)( AUTHOR-Description ) : + + Ruben Smits - Added output for jacobian and jntarray 06/2007 + + *************************************************************************** + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Lesser General Public * + * License as published by the Free Software Foundation; either * + * version 2.1 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with this library; if not, write to the Free Software * + * Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307 USA * + * * + ***************************************************************************/ +/** +// +// \file +// Defines routines for I/O of Frame and related objects. +// \verbatim +// Spaces, tabs and newlines do not have any importance. +// Comments are allowed C-style,C++-style, make/perl/csh -style +// Description of the I/O : +// Vector : OUTPUT : e.g. [10,20,30] +// INPUT : +// 1) [10,20,30] +// 2) Zero +// Twist : e.g. [1,2,3,4,5,6] +// where [1,2,3] is velocity vector +// where [4,5,6] is rotational velocity vector +// Wrench : e.g. [1,2,3,4,5,6] +// where [1,2,3] represents a force vector +// where [4,5,6] represents a torque vector +// Rotation : output : +// [1,2,3; +// 4,5,6; +// 7,8,9] cfr definition of Rotation object. +// input : +// 1) like the output +// 2) EulerZYX,EulerZYZ,RPY word followed by a vector, e.g. : +// Eulerzyx[10,20,30] +// (ANGLES are always expressed in DEGREES for I/O) +// (ANGELS are always expressed in RADIANS for internal representation) +// 3) Rot [1,2,3] [20] Rotates around axis [1,2,3] with an angle +// of 20 degrees. +// 4) Identity returns identity rotation matrix. +// Frames : output : [ Rotationmatrix positionvector ] +// e.g. [ [1,0,0;0,1,0;0,0,1] [1,2,3] ] +// Input : +// 1) [ Rotationmatrix positionvector ] +// 2) DH [ 10,10,50,30] Denavit-Hartenberg representation +// ( is in fact not the representation of a Frame, but more +// limited, cfr. documentation of Frame object.) +// \endverbatim +// +// \warning +// You can use iostream.h or iostream header files for file I/O, +// if one declares the define WANT_STD_IOSTREAM then the standard C++ +// iostreams headers are included instead of the compiler-dependent version +// + * + ****************************************************************************/ +#ifndef FRAMES_IO_H +#define FRAMES_IO_H + +#include "utilities/utility_io.h" +#include "frames.hpp" +#include "jntarray.hpp" +#include "jacobian.hpp" + +namespace KDL { + + //! width to be used when printing variables out with frames_io.h + //! global variable, can be changed. + + + // I/O to C++ stream. + std::ostream& operator << (std::ostream& os,const Vector& v); + std::ostream& operator << (std::ostream& os,const Rotation& R); + std::ostream& operator << (std::ostream& os,const Frame& T); + std::ostream& operator << (std::ostream& os,const Twist& T); + std::ostream& operator << (std::ostream& os,const Wrench& T); + std::ostream& operator << (std::ostream& os,const Vector2& v); + std::ostream& operator << (std::ostream& os,const Rotation2& R); + std::ostream& operator << (std::ostream& os,const Frame2& T); + + + + std::istream& operator >> (std::istream& is,Vector& v); + std::istream& operator >> (std::istream& is,Rotation& R); + std::istream& operator >> (std::istream& is,Frame& T); + std::istream& operator >> (std::istream& os,Twist& T); + std::istream& operator >> (std::istream& os,Wrench& T); + std::istream& operator >> (std::istream& is,Vector2& v); + std::istream& operator >> (std::istream& is,Rotation2& R); + std::istream& operator >> (std::istream& is,Frame2& T); + + +} // namespace Frame + +#endif diff --git a/intern/itasc/kdl/framevel.cpp b/intern/itasc/kdl/framevel.cpp new file mode 100644 index 00000000000..f70bef2e923 --- /dev/null +++ b/intern/itasc/kdl/framevel.cpp @@ -0,0 +1,27 @@ +/***************************************************************************** + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id$ + * $Name: $ + ****************************************************************************/ + + +#include "framevel.hpp" + +namespace KDL { + +#ifndef KDL_INLINE + #include "framevel.inl" +#endif + + + +} diff --git a/intern/itasc/kdl/framevel.hpp b/intern/itasc/kdl/framevel.hpp new file mode 100644 index 00000000000..21a7844f522 --- /dev/null +++ b/intern/itasc/kdl/framevel.hpp @@ -0,0 +1,382 @@ +/***************************************************************************** + * \file + * This file contains the definition of classes for a + * Rall Algebra of (subset of) the classes defined in frames, + * i.e. classes that contain a pair (value,derivative) and define operations on that pair + * this classes are usefull for automatic differentiation ( <-> symbolic diff , <-> numeric diff) + * Defines VectorVel, RotationVel, FrameVel. Look at Frames.h for details on how to work + * with Frame objects. + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id: framevel.hpp 19905 2009-04-23 13:29:54Z ben2610 $ + * $Name: $ + ****************************************************************************/ + +#ifndef KDL_FRAMEVEL_H +#define KDL_FRAMEVEL_H + +#include "utilities/utility.h" +#include "utilities/rall1d.h" +#include "utilities/traits.h" + +#include "frames.hpp" + + + +namespace KDL { + +typedef Rall1d<double> doubleVel; + +IMETHOD doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0) { + return doubleVel((b.t-a.t)/dt,(b.grad-a.grad)/dt); +} + +IMETHOD doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0) { + return doubleVel(a.t+da.t*dt,a.grad+da.grad*dt); +} + +IMETHOD void random(doubleVel& F) { + random(F.t); + random(F.grad); +} +IMETHOD void posrandom(doubleVel& F) { + posrandom(F.t); + posrandom(F.grad); +} + +} + +template <> +struct Traits<KDL::doubleVel> { + typedef double valueType; + typedef KDL::doubleVel derivType; +}; + +namespace KDL { + +class TwistVel; +class VectorVel; +class FrameVel; +class RotationVel; + +class VectorVel +// = TITLE +// An VectorVel is a Vector and its first derivative +// = CLASS TYPE +// Concrete +{ +public: + Vector p; // position vector + Vector v; // velocity vector +public: + VectorVel():p(),v(){} + VectorVel(const Vector& _p,const Vector& _v):p(_p),v(_v) {} + explicit VectorVel(const Vector& _p):p(_p),v(Vector::Zero()) {} + + Vector value() const { return p;} + Vector deriv() const { return v;} + + IMETHOD VectorVel& operator = (const VectorVel& arg); + IMETHOD VectorVel& operator = (const Vector& arg); + IMETHOD VectorVel& operator += (const VectorVel& arg); + IMETHOD VectorVel& operator -= (const VectorVel& arg); + IMETHOD static VectorVel Zero(); + IMETHOD void ReverseSign(); + IMETHOD doubleVel Norm() const; + IMETHOD friend VectorVel operator + (const VectorVel& r1,const VectorVel& r2); + IMETHOD friend VectorVel operator - (const VectorVel& r1,const VectorVel& r2); + IMETHOD friend VectorVel operator + (const Vector& r1,const VectorVel& r2); + IMETHOD friend VectorVel operator - (const Vector& r1,const VectorVel& r2); + IMETHOD friend VectorVel operator + (const VectorVel& r1,const Vector& r2); + IMETHOD friend VectorVel operator - (const VectorVel& r1,const Vector& r2); + IMETHOD friend VectorVel operator * (const VectorVel& r1,const VectorVel& r2); + IMETHOD friend VectorVel operator * (const VectorVel& r1,const Vector& r2); + IMETHOD friend VectorVel operator * (const Vector& r1,const VectorVel& r2); + IMETHOD friend VectorVel operator * (const VectorVel& r1,double r2); + IMETHOD friend VectorVel operator * (double r1,const VectorVel& r2); + IMETHOD friend VectorVel operator * (const doubleVel& r1,const VectorVel& r2); + IMETHOD friend VectorVel operator * (const VectorVel& r2,const doubleVel& r1); + IMETHOD friend VectorVel operator*(const Rotation& R,const VectorVel& x); + + IMETHOD friend VectorVel operator / (const VectorVel& r1,double r2); + IMETHOD friend VectorVel operator / (const VectorVel& r2,const doubleVel& r1); + IMETHOD friend void SetToZero(VectorVel& v); + + + IMETHOD friend bool Equal(const VectorVel& r1,const VectorVel& r2,double eps=epsilon); + IMETHOD friend bool Equal(const Vector& r1,const VectorVel& r2,double eps=epsilon); + IMETHOD friend bool Equal(const VectorVel& r1,const Vector& r2,double eps=epsilon); + IMETHOD friend VectorVel operator - (const VectorVel& r); + IMETHOD friend doubleVel dot(const VectorVel& lhs,const VectorVel& rhs); + IMETHOD friend doubleVel dot(const VectorVel& lhs,const Vector& rhs); + IMETHOD friend doubleVel dot(const Vector& lhs,const VectorVel& rhs); +}; + + + +class RotationVel +// = TITLE +// An RotationVel is a Rotation and its first derivative, a rotation vector +// = CLASS TYPE +// Concrete +{ +public: + Rotation R; // Rotation matrix + Vector w; // rotation vector +public: + RotationVel():R(),w() {} + explicit RotationVel(const Rotation& _R):R(_R),w(Vector::Zero()){} + RotationVel(const Rotation& _R,const Vector& _w):R(_R),w(_w){} + + + Rotation value() const { return R;} + Vector deriv() const { return w;} + + + IMETHOD RotationVel& operator = (const RotationVel& arg); + IMETHOD RotationVel& operator = (const Rotation& arg); + IMETHOD VectorVel UnitX() const; + IMETHOD VectorVel UnitY() const; + IMETHOD VectorVel UnitZ() const; + IMETHOD static RotationVel Identity(); + IMETHOD RotationVel Inverse() const; + IMETHOD VectorVel Inverse(const VectorVel& arg) const; + IMETHOD VectorVel Inverse(const Vector& arg) const; + IMETHOD VectorVel operator*(const VectorVel& arg) const; + IMETHOD VectorVel operator*(const Vector& arg) const; + IMETHOD void DoRotX(const doubleVel& angle); + IMETHOD void DoRotY(const doubleVel& angle); + IMETHOD void DoRotZ(const doubleVel& angle); + IMETHOD static RotationVel RotX(const doubleVel& angle); + IMETHOD static RotationVel RotY(const doubleVel& angle); + IMETHOD static RotationVel RotZ(const doubleVel& angle); + IMETHOD static RotationVel Rot(const Vector& rotvec,const doubleVel& angle); + // rotvec has arbitrary norm + // rotation around a constant vector ! + IMETHOD static RotationVel Rot2(const Vector& rotvec,const doubleVel& angle); + // rotvec is normalized. + // rotation around a constant vector ! + IMETHOD friend RotationVel operator* (const RotationVel& r1,const RotationVel& r2); + IMETHOD friend RotationVel operator* (const Rotation& r1,const RotationVel& r2); + IMETHOD friend RotationVel operator* (const RotationVel& r1,const Rotation& r2); + IMETHOD friend bool Equal(const RotationVel& r1,const RotationVel& r2,double eps=epsilon); + IMETHOD friend bool Equal(const Rotation& r1,const RotationVel& r2,double eps=epsilon); + IMETHOD friend bool Equal(const RotationVel& r1,const Rotation& r2,double eps=epsilon); + + IMETHOD TwistVel Inverse(const TwistVel& arg) const; + IMETHOD TwistVel Inverse(const Twist& arg) const; + IMETHOD TwistVel operator * (const TwistVel& arg) const; + IMETHOD TwistVel operator * (const Twist& arg) const; +}; + + + + +class FrameVel +// = TITLE +// An FrameVel is a Frame and its first derivative, a Twist vector +// = CLASS TYPE +// Concrete +// = CAVEATS +// +{ +public: + RotationVel M; + VectorVel p; +public: + FrameVel(){} + + explicit FrameVel(const Frame& _T): + M(_T.M),p(_T.p) {} + + FrameVel(const Frame& _T,const Twist& _t): + M(_T.M,_t.rot),p(_T.p,_t.vel) {} + + FrameVel(const RotationVel& _M,const VectorVel& _p): + M(_M),p(_p) {} + + + Frame value() const { return Frame(M.value(),p.value());} + Twist deriv() const { return Twist(p.deriv(),M.deriv());} + + + IMETHOD FrameVel& operator = (const Frame& arg); + IMETHOD FrameVel& operator = (const FrameVel& arg); + IMETHOD static FrameVel Identity(); + IMETHOD FrameVel Inverse() const; + IMETHOD VectorVel Inverse(const VectorVel& arg) const; + IMETHOD VectorVel operator*(const VectorVel& arg) const; + IMETHOD VectorVel operator*(const Vector& arg) const; + IMETHOD VectorVel Inverse(const Vector& arg) const; + IMETHOD Frame GetFrame() const; + IMETHOD Twist GetTwist() const; + IMETHOD friend FrameVel operator * (const FrameVel& f1,const FrameVel& f2); + IMETHOD friend FrameVel operator * (const Frame& f1,const FrameVel& f2); + IMETHOD friend FrameVel operator * (const FrameVel& f1,const Frame& f2); + IMETHOD friend bool Equal(const FrameVel& r1,const FrameVel& r2,double eps=epsilon); + IMETHOD friend bool Equal(const Frame& r1,const FrameVel& r2,double eps=epsilon); + IMETHOD friend bool Equal(const FrameVel& r1,const Frame& r2,double eps=epsilon); + + IMETHOD TwistVel Inverse(const TwistVel& arg) const; + IMETHOD TwistVel Inverse(const Twist& arg) const; + IMETHOD TwistVel operator * (const TwistVel& arg) const; + IMETHOD TwistVel operator * (const Twist& arg) const; +}; + + + + + +//very similar to Wrench class. +class TwistVel +// = TITLE +// This class represents a TwistVel. This is a velocity and rotational velocity together +{ +public: + VectorVel vel; + VectorVel rot; +public: + +// = Constructors + TwistVel():vel(),rot() {}; + TwistVel(const VectorVel& _vel,const VectorVel& _rot):vel(_vel),rot(_rot) {}; + TwistVel(const Twist& p,const Twist& v):vel(p.vel, v.vel), rot( p.rot, v.rot) {}; + TwistVel(const Twist& p):vel(p.vel), rot( p.rot) {}; + + Twist value() const { + return Twist(vel.value(),rot.value()); + } + Twist deriv() const { + return Twist(vel.deriv(),rot.deriv()); + } +// = Operators + IMETHOD TwistVel& operator-=(const TwistVel& arg); + IMETHOD TwistVel& operator+=(const TwistVel& arg); + +// = External operators + IMETHOD friend TwistVel operator*(const TwistVel& lhs,double rhs); + IMETHOD friend TwistVel operator*(double lhs,const TwistVel& rhs); + IMETHOD friend TwistVel operator/(const TwistVel& lhs,double rhs); + + IMETHOD friend TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs); + IMETHOD friend TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs); + IMETHOD friend TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs); + + IMETHOD friend TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs); + IMETHOD friend TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs); + IMETHOD friend TwistVel operator-(const TwistVel& arg); + IMETHOD friend void SetToZero(TwistVel& v); + + +// = Zero + static IMETHOD TwistVel Zero(); + +// = Reverse Sign + IMETHOD void ReverseSign(); + +// = Change Reference point + IMETHOD TwistVel RefPoint(const VectorVel& v_base_AB); + // Changes the reference point of the TwistVel. + // The VectorVel v_base_AB is expressed in the same base as the TwistVel + // The VectorVel v_base_AB is a VectorVel from the old point to + // the new point. + // Complexity : 6M+6A + + // = Equality operators + // do not use operator == because the definition of Equal(.,.) is slightly + // different. It compares whether the 2 arguments are equal in an eps-interval + IMETHOD friend bool Equal(const TwistVel& a,const TwistVel& b,double eps=epsilon); + IMETHOD friend bool Equal(const Twist& a,const TwistVel& b,double eps=epsilon); + IMETHOD friend bool Equal(const TwistVel& a,const Twist& b,double eps=epsilon); + +// = Conversion to other entities + IMETHOD Twist GetTwist() const; + IMETHOD Twist GetTwistDot() const; +// = Friends + friend class RotationVel; + friend class FrameVel; + +}; + +IMETHOD VectorVel diff(const VectorVel& a,const VectorVel& b,double dt=1.0) { + return VectorVel(diff(a.p,b.p,dt),diff(a.v,b.v,dt)); +} + +IMETHOD VectorVel addDelta(const VectorVel& a,const VectorVel&da,double dt=1.0) { + return VectorVel(addDelta(a.p,da.p,dt),addDelta(a.v,da.v,dt)); +} +IMETHOD VectorVel diff(const RotationVel& a,const RotationVel& b,double dt = 1.0) { + return VectorVel(diff(a.R,b.R,dt),diff(a.w,b.w,dt)); +} + +IMETHOD RotationVel addDelta(const RotationVel& a,const VectorVel&da,double dt=1.0) { + return RotationVel(addDelta(a.R,da.p,dt),addDelta(a.w,da.v,dt)); +} + +IMETHOD TwistVel diff(const FrameVel& a,const FrameVel& b,double dt=1.0) { + return TwistVel(diff(a.M,b.M,dt),diff(a.p,b.p,dt)); +} + +IMETHOD FrameVel addDelta(const FrameVel& a,const TwistVel& da,double dt=1.0) { + return FrameVel( + addDelta(a.M,da.rot,dt), + addDelta(a.p,da.vel,dt) + ); +} + +IMETHOD void random(VectorVel& a) { + random(a.p); + random(a.v); +} +IMETHOD void random(TwistVel& a) { + random(a.vel); + random(a.rot); +} + +IMETHOD void random(RotationVel& R) { + random(R.R); + random(R.w); +} + +IMETHOD void random(FrameVel& F) { + random(F.M); + random(F.p); +} +IMETHOD void posrandom(VectorVel& a) { + posrandom(a.p); + posrandom(a.v); +} +IMETHOD void posrandom(TwistVel& a) { + posrandom(a.vel); + posrandom(a.rot); +} + +IMETHOD void posrandom(RotationVel& R) { + posrandom(R.R); + posrandom(R.w); +} + +IMETHOD void posrandom(FrameVel& F) { + posrandom(F.M); + posrandom(F.p); +} + +#ifdef KDL_INLINE +#include "framevel.inl" +#endif + +} // namespace + +#endif + + + + diff --git a/intern/itasc/kdl/framevel.inl b/intern/itasc/kdl/framevel.inl new file mode 100644 index 00000000000..994b3d2028e --- /dev/null +++ b/intern/itasc/kdl/framevel.inl @@ -0,0 +1,534 @@ +/***************************************************************************** + * \file + * provides inline functions of rframes.h + * + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id: framevel.inl 19905 2009-04-23 13:29:54Z ben2610 $ + * $Name: $ + ****************************************************************************/ + + +// Methods and operators related to FrameVelVel +// They all delegate most of the work to RotationVelVel and VectorVelVel +FrameVel& FrameVel::operator = (const FrameVel& arg) { + M=arg.M; + p=arg.p; + return *this; +} + +FrameVel FrameVel::Identity() { + return FrameVel(RotationVel::Identity(),VectorVel::Zero()); +} + + +FrameVel operator *(const FrameVel& lhs,const FrameVel& rhs) +{ + return FrameVel(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); +} +FrameVel operator *(const FrameVel& lhs,const Frame& rhs) +{ + return FrameVel(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); +} +FrameVel operator *(const Frame& lhs,const FrameVel& rhs) +{ + return FrameVel(lhs.M*rhs.M , lhs.M*rhs.p+lhs.p ); +} + +VectorVel FrameVel::operator *(const VectorVel & arg) const +{ + return M*arg+p; +} +VectorVel FrameVel::operator *(const Vector & arg) const +{ + return M*arg+p; +} + +VectorVel FrameVel::Inverse(const VectorVel& arg) const +{ + return M.Inverse(arg-p); +} + +VectorVel FrameVel::Inverse(const Vector& arg) const +{ + return M.Inverse(arg-p); +} + +FrameVel FrameVel::Inverse() const +{ + return FrameVel(M.Inverse(),-M.Inverse(p)); +} + +FrameVel& FrameVel::operator = (const Frame& arg) { + M = arg.M; + p = arg.p; + return *this; +} +bool Equal(const FrameVel& r1,const FrameVel& r2,double eps) { + return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); +} +bool Equal(const Frame& r1,const FrameVel& r2,double eps) { + return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); +} +bool Equal(const FrameVel& r1,const Frame& r2,double eps) { + return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); +} + +Frame FrameVel::GetFrame() const { + return Frame(M.R,p.p); +} + +Twist FrameVel::GetTwist() const { + return Twist(p.v,M.w); +} + + +RotationVel operator* (const RotationVel& r1,const RotationVel& r2) { + return RotationVel( r1.R*r2.R, r1.w + r1.R*r2.w ); +} + +RotationVel operator* (const Rotation& r1,const RotationVel& r2) { + return RotationVel( r1*r2.R, r1*r2.w ); +} + +RotationVel operator* (const RotationVel& r1,const Rotation& r2) { + return RotationVel( r1.R*r2, r1.w ); +} + +RotationVel& RotationVel::operator = (const RotationVel& arg) { + R=arg.R; + w=arg.w; + return *this; + } +RotationVel& RotationVel::operator = (const Rotation& arg) { + R=arg; + w=Vector::Zero(); + return *this; +} + +VectorVel RotationVel::UnitX() const { + return VectorVel(R.UnitX(),w*R.UnitX()); +} + +VectorVel RotationVel::UnitY() const { + return VectorVel(R.UnitY(),w*R.UnitY()); +} + +VectorVel RotationVel::UnitZ() const { + return VectorVel(R.UnitZ(),w*R.UnitZ()); +} + + + +RotationVel RotationVel::Identity() { + return RotationVel(Rotation::Identity(),Vector::Zero()); +} + +RotationVel RotationVel::Inverse() const { + return RotationVel(R.Inverse(),-R.Inverse(w)); +} + +VectorVel RotationVel::Inverse(const VectorVel& arg) const { + Vector tmp=R.Inverse(arg.p); + return VectorVel(tmp, + R.Inverse(arg.v-w*arg.p) + ); +} + +VectorVel RotationVel::Inverse(const Vector& arg) const { + Vector tmp=R.Inverse(arg); + return VectorVel(tmp, + R.Inverse(-w*arg) + ); +} + + +VectorVel RotationVel::operator*(const VectorVel& arg) const { + Vector tmp=R*arg.p; + return VectorVel(tmp,w*tmp+R*arg.v); +} + +VectorVel RotationVel::operator*(const Vector& arg) const { + Vector tmp=R*arg; + return VectorVel(tmp,w*tmp); +} + + +// = Rotations +// The Rot... static functions give the value of the appropriate rotation matrix back. +// The DoRot... functions apply a rotation R to *this,such that *this = *this * R. + +void RotationVel::DoRotX(const doubleVel& angle) { + w+=R*Vector(angle.grad,0,0); + R.DoRotX(angle.t); +} +RotationVel RotationVel::RotX(const doubleVel& angle) { + return RotationVel(Rotation::RotX(angle.t),Vector(angle.grad,0,0)); +} + +void RotationVel::DoRotY(const doubleVel& angle) { + w+=R*Vector(0,angle.grad,0); + R.DoRotY(angle.t); +} +RotationVel RotationVel::RotY(const doubleVel& angle) { + return RotationVel(Rotation::RotX(angle.t),Vector(0,angle.grad,0)); +} + +void RotationVel::DoRotZ(const doubleVel& angle) { + w+=R*Vector(0,0,angle.grad); + R.DoRotZ(angle.t); +} +RotationVel RotationVel::RotZ(const doubleVel& angle) { + return RotationVel(Rotation::RotZ(angle.t),Vector(0,0,angle.grad)); +} + + +RotationVel RotationVel::Rot(const Vector& rotvec,const doubleVel& angle) +// rotvec has arbitrary norm +// rotation around a constant vector ! +{ + Vector v(rotvec); + v.Normalize(); + return RotationVel(Rotation::Rot2(v,angle.t),v*angle.grad); +} + +RotationVel RotationVel::Rot2(const Vector& rotvec,const doubleVel& angle) + // rotvec is normalized. +{ + return RotationVel(Rotation::Rot2(rotvec,angle.t),rotvec*angle.grad); +} + + +VectorVel operator + (const VectorVel& r1,const VectorVel& r2) { + return VectorVel(r1.p+r2.p,r1.v+r2.v); +} + +VectorVel operator - (const VectorVel& r1,const VectorVel& r2) { + return VectorVel(r1.p-r2.p,r1.v-r2.v); +} + +VectorVel operator + (const VectorVel& r1,const Vector& r2) { + return VectorVel(r1.p+r2,r1.v); +} + +VectorVel operator - (const VectorVel& r1,const Vector& r2) { + return VectorVel(r1.p-r2,r1.v); +} + +VectorVel operator + (const Vector& r1,const VectorVel& r2) { + return VectorVel(r1+r2.p,r2.v); +} + +VectorVel operator - (const Vector& r1,const VectorVel& r2) { + return VectorVel(r1-r2.p,-r2.v); +} + +// unary - +VectorVel operator - (const VectorVel& r) { + return VectorVel(-r.p,-r.v); +} + +void SetToZero(VectorVel& v){ + SetToZero(v.p); + SetToZero(v.v); +} + +// cross prod. +VectorVel operator * (const VectorVel& r1,const VectorVel& r2) { + return VectorVel(r1.p*r2.p, r1.p*r2.v+r1.v*r2.p); +} + +VectorVel operator * (const VectorVel& r1,const Vector& r2) { + return VectorVel(r1.p*r2, r1.v*r2); +} + +VectorVel operator * (const Vector& r1,const VectorVel& r2) { + return VectorVel(r1*r2.p, r1*r2.v); +} + + + +// scalar mult. +VectorVel operator * (double r1,const VectorVel& r2) { + return VectorVel(r1*r2.p, r1*r2.v); +} + +VectorVel operator * (const VectorVel& r1,double r2) { + return VectorVel(r1.p*r2, r1.v*r2); +} + + + +VectorVel operator * (const doubleVel& r1,const VectorVel& r2) { + return VectorVel(r1.t*r2.p, r1.t*r2.v + r1.grad*r2.p); +} + +VectorVel operator * (const VectorVel& r2,const doubleVel& r1) { + return VectorVel(r1.t*r2.p, r1.t*r2.v + r1.grad*r2.p); +} + +VectorVel operator / (const VectorVel& r1,double r2) { + return VectorVel(r1.p/r2, r1.v/r2); +} + +VectorVel operator / (const VectorVel& r2,const doubleVel& r1) { + return VectorVel(r2.p/r1.t, r2.v/r1.t - r2.p*r1.grad/r1.t/r1.t); +} + +VectorVel operator*(const Rotation& R,const VectorVel& x) { + return VectorVel(R*x.p,R*x.v); +} + +VectorVel& VectorVel::operator = (const VectorVel& arg) { + p=arg.p; + v=arg.v; + return *this; +} +VectorVel& VectorVel::operator = (const Vector& arg) { + p=arg; + v=Vector::Zero(); + return *this; +} +VectorVel& VectorVel::operator += (const VectorVel& arg) { + p+=arg.p; + v+=arg.v; + return *this; +} +VectorVel& VectorVel::operator -= (const VectorVel& arg) { + p-=arg.p; + v-=arg.v; + return *this; +} + +VectorVel VectorVel::Zero() { + return VectorVel(Vector::Zero(),Vector::Zero()); +} +void VectorVel::ReverseSign() { + p.ReverseSign(); + v.ReverseSign(); +} +doubleVel VectorVel::Norm() const { + double n = p.Norm(); + return doubleVel(n,dot(p,v)/n); +} + +bool Equal(const VectorVel& r1,const VectorVel& r2,double eps) { + return (Equal(r1.p,r2.p,eps) && Equal(r1.v,r2.v,eps)); +} +bool Equal(const Vector& r1,const VectorVel& r2,double eps) { + return (Equal(r1,r2.p,eps) && Equal(Vector::Zero(),r2.v,eps)); +} +bool Equal(const VectorVel& r1,const Vector& r2,double eps) { + return (Equal(r1.p,r2,eps) && Equal(r1.v,Vector::Zero(),eps)); +} + +bool Equal(const RotationVel& r1,const RotationVel& r2,double eps) { + return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps)); +} +bool Equal(const Rotation& r1,const RotationVel& r2,double eps) { + return (Equal(Vector::Zero(),r2.w,eps) && Equal(r1,r2.R,eps)); +} +bool Equal(const RotationVel& r1,const Rotation& r2,double eps) { + return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps)); +} +bool Equal(const TwistVel& a,const TwistVel& b,double eps) { + return (Equal(a.rot,b.rot,eps)&& + Equal(a.vel,b.vel,eps) ); +} +bool Equal(const Twist& a,const TwistVel& b,double eps) { + return (Equal(a.rot,b.rot,eps)&& + Equal(a.vel,b.vel,eps) ); +} +bool Equal(const TwistVel& a,const Twist& b,double eps) { + return (Equal(a.rot,b.rot,eps)&& + Equal(a.vel,b.vel,eps) ); +} + + + +IMETHOD doubleVel dot(const VectorVel& lhs,const VectorVel& rhs) { + return doubleVel(dot(lhs.p,rhs.p),dot(lhs.p,rhs.v)+dot(lhs.v,rhs.p)); +} +IMETHOD doubleVel dot(const VectorVel& lhs,const Vector& rhs) { + return doubleVel(dot(lhs.p,rhs),dot(lhs.v,rhs)); +} +IMETHOD doubleVel dot(const Vector& lhs,const VectorVel& rhs) { + return doubleVel(dot(lhs,rhs.p),dot(lhs,rhs.v)); +} + + + + + + + + + + + + +TwistVel TwistVel::Zero() +{ + return TwistVel(VectorVel::Zero(),VectorVel::Zero()); +} + + +void TwistVel::ReverseSign() +{ + vel.ReverseSign(); + rot.ReverseSign(); +} + +TwistVel TwistVel::RefPoint(const VectorVel& v_base_AB) + // Changes the reference point of the TwistVel. + // The VectorVel v_base_AB is expressed in the same base as the TwistVel + // The VectorVel v_base_AB is a VectorVel from the old point to + // the new point. + // Complexity : 6M+6A +{ + return TwistVel(this->vel+this->rot*v_base_AB,this->rot); +} + +TwistVel& TwistVel::operator-=(const TwistVel& arg) +{ + vel-=arg.vel; + rot -=arg.rot; + return *this; +} + +TwistVel& TwistVel::operator+=(const TwistVel& arg) +{ + vel+=arg.vel; + rot +=arg.rot; + return *this; +} + + +TwistVel operator*(const TwistVel& lhs,double rhs) +{ + return TwistVel(lhs.vel*rhs,lhs.rot*rhs); +} + +TwistVel operator*(double lhs,const TwistVel& rhs) +{ + return TwistVel(lhs*rhs.vel,lhs*rhs.rot); +} + +TwistVel operator/(const TwistVel& lhs,double rhs) +{ + return TwistVel(lhs.vel/rhs,lhs.rot/rhs); +} + + +TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs) +{ + return TwistVel(lhs.vel*rhs,lhs.rot*rhs); +} + +TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs) +{ + return TwistVel(lhs*rhs.vel,lhs*rhs.rot); +} + +TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs) +{ + return TwistVel(lhs.vel/rhs,lhs.rot/rhs); +} + + + +// addition of TwistVel's +TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs) +{ + return TwistVel(lhs.vel+rhs.vel,lhs.rot+rhs.rot); +} + +TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs) +{ + return TwistVel(lhs.vel-rhs.vel,lhs.rot-rhs.rot); +} + +// unary - +TwistVel operator-(const TwistVel& arg) +{ + return TwistVel(-arg.vel,-arg.rot); +} + +void SetToZero(TwistVel& v) +{ + SetToZero(v.vel); + SetToZero(v.rot); +} + + + + + +TwistVel RotationVel::Inverse(const TwistVel& arg) const +{ + return TwistVel(Inverse(arg.vel),Inverse(arg.rot)); +} + +TwistVel RotationVel::operator * (const TwistVel& arg) const +{ + return TwistVel((*this)*arg.vel,(*this)*arg.rot); +} + +TwistVel RotationVel::Inverse(const Twist& arg) const +{ + return TwistVel(Inverse(arg.vel),Inverse(arg.rot)); +} + +TwistVel RotationVel::operator * (const Twist& arg) const +{ + return TwistVel((*this)*arg.vel,(*this)*arg.rot); +} + + +TwistVel FrameVel::operator * (const TwistVel& arg) const +{ + TwistVel tmp; + tmp.rot = M*arg.rot; + tmp.vel = M*arg.vel+p*tmp.rot; + return tmp; +} + +TwistVel FrameVel::operator * (const Twist& arg) const +{ + TwistVel tmp; + tmp.rot = M*arg.rot; + tmp.vel = M*arg.vel+p*tmp.rot; + return tmp; +} + +TwistVel FrameVel::Inverse(const TwistVel& arg) const +{ + TwistVel tmp; + tmp.rot = M.Inverse(arg.rot); + tmp.vel = M.Inverse(arg.vel-p*arg.rot); + return tmp; +} + +TwistVel FrameVel::Inverse(const Twist& arg) const +{ + TwistVel tmp; + tmp.rot = M.Inverse(arg.rot); + tmp.vel = M.Inverse(arg.vel-p*arg.rot); + return tmp; +} + +Twist TwistVel::GetTwist() const { + return Twist(vel.p,rot.p); +} + +Twist TwistVel::GetTwistDot() const { + return Twist(vel.v,rot.v); +} diff --git a/intern/itasc/kdl/inertia.cpp b/intern/itasc/kdl/inertia.cpp new file mode 100644 index 00000000000..6c7337d0dc4 --- /dev/null +++ b/intern/itasc/kdl/inertia.cpp @@ -0,0 +1,48 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "inertia.hpp" + +#include <Eigen/Array> + +namespace KDL { +using namespace Eigen; + +Inertia::Inertia(double m,double Ixx,double Iyy,double Izz,double Ixy,double Ixz,double Iyz): +data(Matrix<double,6,6>::Zero()) +{ + data(0,0)=Ixx; + data(1,1)=Iyy; + data(2,2)=Izz; + data(2,1)=data(1,2)=Ixy; + data(3,1)=data(1,3)=Ixz; + data(3,2)=data(2,3)=Iyz; + + data.block(3,3,3,3)=m*Matrix<double,3,3>::Identity(); +} + +Inertia::~Inertia() +{ +} + + + +} diff --git a/intern/itasc/kdl/inertia.hpp b/intern/itasc/kdl/inertia.hpp new file mode 100644 index 00000000000..9f33859671c --- /dev/null +++ b/intern/itasc/kdl/inertia.hpp @@ -0,0 +1,70 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDLINERTIA_HPP +#define KDLINERTIA_HPP + +#include <Eigen/Array> +#include "frames.hpp" + +namespace KDL { + +using namespace Eigen; + +/** + * This class offers the inertia-structure of a body + * An inertia is defined in a certain reference point and a certain reference base. + * The reference point does not have to coincide with the origin of the reference frame. + */ +class Inertia{ +public: + + /** + * This constructor creates a cartesian space inertia matrix, + * the arguments are the mass and the inertia moments in the cog. + */ + Inertia(double m=0,double Ixx=0,double Iyy=0,double Izz=0,double Ixy=0,double Ixz=0,double Iyz=0); + + static inline Inertia Zero(){ + return Inertia(0,0,0,0,0,0,0); + }; + + friend class Rotation; + friend class Frame; + + /** + * F = m*a + */ + // Wrench operator* (const AccelerationTwist& acc); + + + ~Inertia(); +private: + Matrix<double,6,6,RowMajor> data; + +}; + + + + +} + +#endif diff --git a/intern/itasc/kdl/jacobian.cpp b/intern/itasc/kdl/jacobian.cpp new file mode 100644 index 00000000000..f8f46b32619 --- /dev/null +++ b/intern/itasc/kdl/jacobian.cpp @@ -0,0 +1,129 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "jacobian.hpp" + +namespace KDL +{ + Jacobian::Jacobian(unsigned int _size,unsigned int _nr_blocks): + size(_size),nr_blocks(_nr_blocks) + { + twists = new Twist[size*nr_blocks]; + } + + Jacobian::Jacobian(const Jacobian& arg): + size(arg.columns()), + nr_blocks(arg.nr_blocks) + { + twists = new Twist[size*nr_blocks]; + for(unsigned int i=0;i<size*nr_blocks;i++) + twists[i] = arg.twists[i]; + } + + Jacobian& Jacobian::operator = (const Jacobian& arg) + { + assert(size==arg.size); + assert(nr_blocks==arg.nr_blocks); + for(unsigned int i=0;i<size;i++) + twists[i]=arg.twists[i]; + return *this; + } + + + Jacobian::~Jacobian() + { + delete [] twists; + } + + double Jacobian::operator()(int i,int j)const + { + assert(i<6*(int)nr_blocks&&j<(int)size); + return twists[j+6*(int)(floor((double)i/6))](i%6); + } + + double& Jacobian::operator()(int i,int j) + { + assert(i<6*(int)nr_blocks&&j<(int)size); + return twists[j+6*(int)(floor((double)i/6))](i%6); + } + + unsigned int Jacobian::rows()const + { + return 6*nr_blocks; + } + + unsigned int Jacobian::columns()const + { + return size; + } + + void SetToZero(Jacobian& jac) + { + for(unsigned int i=0;i<jac.size*jac.nr_blocks;i++) + SetToZero(jac.twists[i]); + } + + void changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest) + { + assert(src1.size==dest.size); + assert(src1.nr_blocks==dest.nr_blocks); + for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++) + dest.twists[i]=src1.twists[i].RefPoint(base_AB); + } + + void changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest) + { + assert(src1.size==dest.size); + assert(src1.nr_blocks==dest.nr_blocks); + for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++) + dest.twists[i]=rot*src1.twists[i]; + } + + void changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest) + { + assert(src1.size==dest.size); + assert(src1.nr_blocks==dest.nr_blocks); + for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++) + dest.twists[i]=frame*src1.twists[i]; + } + + bool Jacobian::operator ==(const Jacobian& arg) + { + return Equal((*this),arg); + } + + bool Jacobian::operator!=(const Jacobian& arg) + { + return !Equal((*this),arg); + } + + bool Equal(const Jacobian& a,const Jacobian& b,double eps) + { + if(a.rows()==b.rows()&&a.columns()==b.columns()){ + bool rc=true; + for(unsigned int i=0;i<a.columns();i++) + rc&=Equal(a.twists[i],b.twists[i],eps); + return rc; + }else + return false; + } + +} diff --git a/intern/itasc/kdl/jacobian.hpp b/intern/itasc/kdl/jacobian.hpp new file mode 100644 index 00000000000..e9057451c9f --- /dev/null +++ b/intern/itasc/kdl/jacobian.hpp @@ -0,0 +1,68 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_JACOBIAN_HPP +#define KDL_JACOBIAN_HPP + +#include "frames.hpp" + +namespace KDL +{ + //Forward declaration + class ChainJntToJacSolver; + + class Jacobian + { + friend class ChainJntToJacSolver; + private: + unsigned int size; + unsigned int nr_blocks; + public: + Twist* twists; + Jacobian(unsigned int size,unsigned int nr=1); + Jacobian(const Jacobian& arg); + + Jacobian& operator=(const Jacobian& arg); + + bool operator ==(const Jacobian& arg); + bool operator !=(const Jacobian& arg); + + friend bool Equal(const Jacobian& a,const Jacobian& b,double eps=epsilon); + + + ~Jacobian(); + + double operator()(int i,int j)const; + double& operator()(int i,int j); + unsigned int rows()const; + unsigned int columns()const; + + friend void SetToZero(Jacobian& jac); + + friend void changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest); + friend void changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest); + friend void changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest); + + + }; +} + +#endif diff --git a/intern/itasc/kdl/jntarray.cpp b/intern/itasc/kdl/jntarray.cpp new file mode 100644 index 00000000000..2adb76081f3 --- /dev/null +++ b/intern/itasc/kdl/jntarray.cpp @@ -0,0 +1,152 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "jntarray.hpp" + +namespace KDL +{ + JntArray::JntArray(): + size(0), + data(NULL) + { + } + + JntArray::JntArray(unsigned int _size): + size(_size) + { + assert(0 < size); + data = new double[size]; + SetToZero(*this); + } + + + JntArray::JntArray(const JntArray& arg): + size(arg.size) + { + data = ((0 < size) ? new double[size] : NULL); + for(unsigned int i=0;i<size;i++) + data[i]=arg.data[i]; + } + + JntArray& JntArray::operator = (const JntArray& arg) + { + assert(size==arg.size); + for(unsigned int i=0;i<size;i++) + data[i]=arg.data[i]; + return *this; + } + + + JntArray::~JntArray() + { + delete [] data; + } + + void JntArray::resize(unsigned int newSize) + { + delete [] data; + size = newSize; + data = new double[size]; + SetToZero(*this); + } + + double JntArray::operator()(unsigned int i,unsigned int j)const + { + assert(i<size&&j==0); + assert(0 != size); // found JntArray containing no data + return data[i]; + } + + double& JntArray::operator()(unsigned int i,unsigned int j) + { + assert(i<size&&j==0); + assert(0 != size); // found JntArray containing no data + return data[i]; + } + + unsigned int JntArray::rows()const + { + return size; + } + + unsigned int JntArray::columns()const + { + return 0; + } + + void Add(const JntArray& src1,const JntArray& src2,JntArray& dest) + { + assert(src1.size==src2.size&&src1.size==dest.size); + for(unsigned int i=0;i<dest.size;i++) + dest.data[i]=src1.data[i]+src2.data[i]; + } + + void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest) + { + assert(src1.size==src2.size&&src1.size==dest.size); + for(unsigned int i=0;i<dest.size;i++) + dest.data[i]=src1.data[i]-src2.data[i]; + } + + void Multiply(const JntArray& src,const double& factor,JntArray& dest) + { + assert(src.size==dest.size); + for(unsigned int i=0;i<dest.size;i++) + dest.data[i]=factor*src.data[i]; + } + + void Divide(const JntArray& src,const double& factor,JntArray& dest) + { + assert(src.rows()==dest.size); + for(unsigned int i=0;i<dest.size;i++) + dest.data[i]=src.data[i]/factor; + } + + void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest) + { + assert(jac.columns()==src.size); + SetToZero(dest); + for(unsigned int i=0;i<6;i++) + for(unsigned int j=0;j<src.size;j++) + dest(i)+=jac(i,j)*src.data[j]; + } + + void SetToZero(JntArray& array) + { + for(unsigned int i=0;i<array.size;i++) + array.data[i]=0; + } + + bool Equal(const JntArray& src1, const JntArray& src2,double eps) + { + assert(src1.size==src2.size); + bool ret = true; + for(unsigned int i=0;i<src1.size;i++) + ret = ret && Equal(src1.data[i],src2.data[i],eps); + return ret; + } + + bool operator==(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);}; + //bool operator!=(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);}; + +} + + diff --git a/intern/itasc/kdl/jntarray.hpp b/intern/itasc/kdl/jntarray.hpp new file mode 100644 index 00000000000..19ffa4343e3 --- /dev/null +++ b/intern/itasc/kdl/jntarray.hpp @@ -0,0 +1,217 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_JNTARRAY_HPP +#define KDL_JNTARRAY_HPP + +#include "frames.hpp" +#include "jacobian.hpp" + +namespace KDL +{ + /** + * @brief This class represents an fixed size array containing + * joint values of a KDL::Chain. + * + * \warning An object constructed with the default constructor provides + * a valid, but inert, object. Many of the member functions will do + * the correct thing and have no affect on this object, but some + * member functions can _NOT_ deal with an inert/empty object. These + * functions will assert() and exit the program instead. The intended use + * case for the default constructor (in an RTT/OCL setting) is outlined in + * code below - the default constructor plus the resize() function allow + * use of JntArray objects whose size is set within a configureHook() call + * (typically based on a size determined from a property). + +\code +class MyTask : public RTT::TaskContext +{ + JntArray j; + MyTask() + {} // invokes j's default constructor + + bool configureHook() + { + unsigned int size = some_property.rvalue(); + j.resize(size) + ... + } + + void updateHook() + { + ** use j here + } +}; +/endcode + + */ + + class JntArray + { + private: + unsigned int size; + double* data; + public: + /** Construct with _no_ data array + * @post NULL == data + * @post 0 == rows() + * @warning use of an object constructed like this, without + * a resize() first, may result in program exit! See class + * documentation. + */ + JntArray(); + /** + * Constructor of the joint array + * + * @param size size of the array, this cannot be changed + * afterwards. + * @pre 0 < size + * @post NULL != data + * @post 0 < rows() + * @post all elements in data have 0 value + */ + JntArray(unsigned int size); + /** Copy constructor + * @note Will correctly copy an empty object + */ + JntArray(const JntArray& arg); + ~JntArray(); + /** Resize the array + * @warning This causes a dynamic allocation (and potentially + * also a dynamic deallocation). This _will_ negatively affect + * real-time performance! + * + * @post newSize == rows() + * @post NULL != data + * @post all elements in data have 0 value + */ + void resize(unsigned int newSize); + + JntArray& operator = ( const JntArray& arg); + /** + * get_item operator for the joint array, if a second value is + * given it should be zero, since a JntArray resembles a column. + * + * + * @return the joint value at position i, starting from 0 + * @pre 0 != size (ie non-default constructor or resize() called) + */ + double operator()(unsigned int i,unsigned int j=0)const; + /** + * set_item operator, again if a second value is given it + *should be zero. + * + * @return reference to the joint value at position i,starting + *from zero. + * @pre 0 != size (ie non-default constructor or resize() called) + */ + double& operator()(unsigned int i,unsigned int j=0); + /** + * Returns the number of rows (size) of the array + * + */ + unsigned int rows()const; + /** + * Returns the number of columns of the array, always 1. + */ + unsigned int columns()const; + + /** + * Function to add two joint arrays, all the arguments must + * have the same size: A + B = C. This function is + * aliasing-safe, A or B can be the same array as C. + * + * @param src1 A + * @param src2 B + * @param dest C + */ + friend void Add(const JntArray& src1,const JntArray& src2,JntArray& dest); + /** + * Function to subtract two joint arrays, all the arguments must + * have the same size: A - B = C. This function is + * aliasing-safe, A or B can be the same array as C. + * + * @param src1 A + * @param src2 B + * @param dest C + */ + friend void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest); + /** + * Function to multiply all the array values with a scalar + * factor: A*b=C. This function is aliasing-safe, A can be the + * same array as C. + * + * @param src A + * @param factor b + * @param dest C + */ + friend void Multiply(const JntArray& src,const double& factor,JntArray& dest); + /** + * Function to divide all the array values with a scalar + * factor: A/b=C. This function is aliasing-safe, A can be the + * same array as C. + * + * @param src A + * @param factor b + * @param dest C + */ + friend void Divide(const JntArray& src,const double& factor,JntArray& dest); + /** + * Function to multiply a KDL::Jacobian with a KDL::JntArray + * to get a KDL::Twist, it should not be used to calculate the + * forward velocity kinematics, the solver classes are built + * for this purpose. + * J*q = t + * + * @param jac J + * @param src q + * @param dest t + * @post dest==Twist::Zero() if 0==src.rows() (ie src is empty) + */ + friend void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest); + /** + * Function to set all the values of the array to 0 + * + * @param array + */ + friend void SetToZero(JntArray& array); + /** + * Function to check if two arrays are the same with a + *precision of eps + * + * @param src1 + * @param src2 + * @param eps default: epsilon + * @return true if each element of src1 is within eps of the same + * element in src2, or if both src1 and src2 have no data (ie 0==rows()) + */ + friend bool Equal(const JntArray& src1,const JntArray& src2,double eps=epsilon); + + friend bool operator==(const JntArray& src1,const JntArray& src2); + //friend bool operator!=(const JntArray& src1,const JntArray& src2); + }; + + bool operator==(const JntArray& src1,const JntArray& src2); + //bool operator!=(const JntArray& src1,const JntArray& src2); + +} + +#endif diff --git a/intern/itasc/kdl/jntarrayacc.cpp b/intern/itasc/kdl/jntarrayacc.cpp new file mode 100644 index 00000000000..3c9c67d9ef9 --- /dev/null +++ b/intern/itasc/kdl/jntarrayacc.cpp @@ -0,0 +1,170 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "jntarrayacc.hpp" + +namespace KDL +{ + JntArrayAcc::JntArrayAcc(unsigned int size): + q(size),qdot(size),qdotdot(size) + { + } + JntArrayAcc::JntArrayAcc(const JntArray& qin, const JntArray& qdotin,const JntArray& qdotdotin): + q(qin),qdot(qdotin),qdotdot(qdotdotin) + { + assert(q.rows()==qdot.rows()&&qdot.rows()==qdotdot.rows()); + } + JntArrayAcc::JntArrayAcc(const JntArray& qin, const JntArray& qdotin): + q(qin),qdot(qdotin),qdotdot(q.rows()) + { + assert(q.rows()==qdot.rows()); + } + JntArrayAcc::JntArrayAcc(const JntArray& qin): + q(qin),qdot(q.rows()),qdotdot(q.rows()) + { + } + + JntArray JntArrayAcc::value()const + { + return q; + } + + JntArray JntArrayAcc::deriv()const + { + return qdot; + } + JntArray JntArrayAcc::dderiv()const + { + return qdotdot; + } + + void Add(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest) + { + Add(src1.q,src2.q,dest.q); + Add(src1.qdot,src2.qdot,dest.qdot); + Add(src1.qdotdot,src2.qdotdot,dest.qdotdot); + } + void Add(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest) + { + Add(src1.q,src2.q,dest.q); + Add(src1.qdot,src2.qdot,dest.qdot); + dest.qdotdot=src1.qdotdot; + } + void Add(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest) + { + Add(src1.q,src2,dest.q); + dest.qdot=src1.qdot; + dest.qdotdot=src1.qdotdot; + } + + void Subtract(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest) + { + Subtract(src1.q,src2.q,dest.q); + Subtract(src1.qdot,src2.qdot,dest.qdot); + Subtract(src1.qdotdot,src2.qdotdot,dest.qdotdot); + } + void Subtract(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest) + { + Subtract(src1.q,src2.q,dest.q); + Subtract(src1.qdot,src2.qdot,dest.qdot); + dest.qdotdot=src1.qdotdot; + } + void Subtract(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest) + { + Subtract(src1.q,src2,dest.q); + dest.qdot=src1.qdot; + dest.qdotdot=src1.qdotdot; + } + + void Multiply(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest) + { + Multiply(src.q,factor,dest.q); + Multiply(src.qdot,factor,dest.qdot); + Multiply(src.qdotdot,factor,dest.qdotdot); + } + void Multiply(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest) + { + Multiply(src.qdot,factor.grad*2,dest.qdot); + Multiply(src.qdotdot,factor.t,dest.qdotdot); + Add(dest.qdot,dest.qdotdot,dest.qdotdot); + Multiply(src.q,factor.grad,dest.q); + Multiply(src.qdot,factor.t,dest.qdot); + Add(dest.qdot,dest.q,dest.qdot); + Multiply(src.q,factor.t,dest.q); + } + void Multiply(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest) + { + Multiply(src.q,factor.dd,dest.q); + Multiply(src.qdot,factor.d*2,dest.qdot); + Multiply(src.qdotdot,factor.t,dest.qdotdot); + Add(dest.qdotdot,dest.qdot,dest.qdotdot); + Add(dest.qdotdot,dest.q,dest.qdotdot); + Multiply(src.q,factor.d,dest.q); + Multiply(src.qdot,factor.t,dest.qdot); + Add(dest.qdot,dest.q,dest.qdot); + Multiply(src.q,factor.t,dest.q); + } + + void Divide(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest) + { + Divide(src.q,factor,dest.q); + Divide(src.qdot,factor,dest.qdot); + Divide(src.qdotdot,factor,dest.qdotdot); + } + void Divide(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest) + { + Multiply(src.q,(2*factor.grad*factor.grad)/(factor.t*factor.t*factor.t),dest.q); + Multiply(src.qdot,(2*factor.grad)/(factor.t*factor.t),dest.qdot); + Divide(src.qdotdot,factor.t,dest.qdotdot); + Subtract(dest.qdotdot,dest.qdot,dest.qdotdot); + Add(dest.qdotdot,dest.q,dest.qdotdot); + Multiply(src.q,factor.grad/(factor.t*factor.t),dest.q); + Divide(src.qdot,factor.t,dest.qdot); + Subtract(dest.qdot,dest.q,dest.qdot); + Divide(src.q,factor.t,dest.q); + } + void Divide(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest) + { + Multiply(src.q,(2*factor.d*factor.d)/(factor.t*factor.t*factor.t)-factor.dd/(factor.t*factor.t),dest.q); + Multiply(src.qdot,(2*factor.d)/(factor.t*factor.t),dest.qdot); + Divide(src.qdotdot,factor.t,dest.qdotdot); + Subtract(dest.qdotdot,dest.qdot,dest.qdotdot); + Add(dest.qdotdot,dest.q,dest.qdotdot); + Multiply(src.q,factor.d/(factor.t*factor.t),dest.q); + Divide(src.qdot,factor.t,dest.qdot); + Subtract(dest.qdot,dest.q,dest.qdot); + Divide(src.q,factor.t,dest.q); + } + + void SetToZero(JntArrayAcc& array) + { + SetToZero(array.q); + SetToZero(array.qdot); + SetToZero(array.qdotdot); + } + + bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps) + { + return (Equal(src1.q,src2.q,eps)&&Equal(src1.qdot,src2.qdot,eps)&&Equal(src1.qdotdot,src2.qdotdot,eps)); + } +} + + diff --git a/intern/itasc/kdl/jntarrayacc.hpp b/intern/itasc/kdl/jntarrayacc.hpp new file mode 100644 index 00000000000..275aa58f21e --- /dev/null +++ b/intern/itasc/kdl/jntarrayacc.hpp @@ -0,0 +1,66 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_JNTARRAYACC_HPP +#define KDL_JNTARRAYACC_HPP + +#include "utilities/utility.h" +#include "jntarray.hpp" +#include "jntarrayvel.hpp" +#include "frameacc.hpp" + +namespace KDL +{ + class JntArrayAcc + { + public: + JntArray q; + JntArray qdot; + JntArray qdotdot; + public: + JntArrayAcc(unsigned int size); + JntArrayAcc(const JntArray& q,const JntArray& qdot,const JntArray& qdotdot); + JntArrayAcc(const JntArray& q,const JntArray& qdot); + JntArrayAcc(const JntArray& q); + + JntArray value()const; + JntArray deriv()const; + JntArray dderiv()const; + + friend void Add(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest); + friend void Add(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest); + friend void Add(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest); + friend void Subtract(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest); + friend void Subtract(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest); + friend void Subtract(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest); + friend void Multiply(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest); + friend void Multiply(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest); + friend void Multiply(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest); + friend void Divide(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest); + friend void Divide(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest); + friend void Divide(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest); + friend void SetToZero(JntArrayAcc& array); + friend bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps=epsilon); + + }; +} + +#endif diff --git a/intern/itasc/kdl/jntarrayvel.cpp b/intern/itasc/kdl/jntarrayvel.cpp new file mode 100644 index 00000000000..df5c7fb0fb3 --- /dev/null +++ b/intern/itasc/kdl/jntarrayvel.cpp @@ -0,0 +1,111 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +#include "jntarrayacc.hpp" + +namespace KDL +{ + JntArrayVel::JntArrayVel(unsigned int size): + q(size),qdot(size) + { + } + JntArrayVel::JntArrayVel(const JntArray& qin, const JntArray& qdotin): + q(qin),qdot(qdotin) + { + assert(q.rows()==qdot.rows()); + } + JntArrayVel::JntArrayVel(const JntArray& qin): + q(qin),qdot(q.rows()) + { + } + + JntArray JntArrayVel::value()const + { + return q; + } + + JntArray JntArrayVel::deriv()const + { + return qdot; + } + + void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest) + { + Add(src1.q,src2.q,dest.q); + Add(src1.qdot,src2.qdot,dest.qdot); + } + void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest) + { + Add(src1.q,src2,dest.q); + dest.qdot=src1.qdot; + } + + void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest) + { + Subtract(src1.q,src2.q,dest.q); + Subtract(src1.qdot,src2.qdot,dest.qdot); + } + void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest) + { + Subtract(src1.q,src2,dest.q); + dest.qdot=src1.qdot; + } + + void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest) + { + Multiply(src.q,factor,dest.q); + Multiply(src.qdot,factor,dest.qdot); + } + void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest) + { + Multiply(src.q,factor.grad,dest.q); + Multiply(src.qdot,factor.t,dest.qdot); + Add(dest.qdot,dest.q,dest.qdot); + Multiply(src.q,factor.t,dest.q); + } + + void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest) + { + Divide(src.q,factor,dest.q); + Divide(src.qdot,factor,dest.qdot); + } + void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest) + { + Multiply(src.q,(factor.grad/factor.t/factor.t),dest.q); + Divide(src.qdot,factor.t,dest.qdot); + Subtract(dest.qdot,dest.q,dest.qdot); + Divide(src.q,factor.t,dest.q); + } + + void SetToZero(JntArrayVel& array) + { + SetToZero(array.q); + SetToZero(array.qdot); + } + + bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps) + { + return Equal(src1.q,src2.q,eps)&&Equal(src1.qdot,src2.qdot,eps); + } +} + + diff --git a/intern/itasc/kdl/jntarrayvel.hpp b/intern/itasc/kdl/jntarrayvel.hpp new file mode 100644 index 00000000000..faa82076ebb --- /dev/null +++ b/intern/itasc/kdl/jntarrayvel.hpp @@ -0,0 +1,59 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_JNTARRAYVEL_HPP +#define KDL_JNTARRAYVEL_HPP + +#include "utilities/utility.h" +#include "jntarray.hpp" +#include "framevel.hpp" + +namespace KDL +{ + + class JntArrayVel + { + public: + JntArray q; + JntArray qdot; + public: + JntArrayVel(unsigned int size); + JntArrayVel(const JntArray& q,const JntArray& qdot); + JntArrayVel(const JntArray& q); + + JntArray value()const; + JntArray deriv()const; + + friend void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest); + friend void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest); + friend void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest); + friend void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest); + friend void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest); + friend void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest); + friend void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest); + friend void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest); + friend void SetToZero(JntArrayVel& array); + friend bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps=epsilon); + + }; +} + +#endif diff --git a/intern/itasc/kdl/joint.cpp b/intern/itasc/kdl/joint.cpp new file mode 100644 index 00000000000..dc5f17e5bf7 --- /dev/null +++ b/intern/itasc/kdl/joint.cpp @@ -0,0 +1,153 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "joint.hpp" + +namespace KDL { + + Joint::Joint(const JointType& _type, const double& _scale, const double& _offset, + const double& _inertia, const double& _damping, const double& _stiffness): + type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness) + { + // for sphere and swing, offset is not used, assume no offset + } + + Joint::Joint(const Joint& in): + type(in.type),scale(in.scale),offset(in.offset), + inertia(in.inertia),damping(in.damping),stiffness(in.stiffness) + { + } + + Joint& Joint::operator=(const Joint& in) + { + type=in.type; + scale=in.scale; + offset=in.offset; + inertia=in.inertia; + damping=in.damping; + stiffness=in.stiffness; + return *this; + } + + + Joint::~Joint() + { + } + + Frame Joint::pose(const double& q)const + { + + switch(type){ + case RotX: + return Frame(Rotation::RotX(scale*q+offset)); + break; + case RotY: + return Frame(Rotation::RotY(scale*q+offset)); + break; + case RotZ: + return Frame(Rotation::RotZ(scale*q+offset)); + break; + case TransX: + return Frame(Vector(scale*q+offset,0.0,0.0)); + break; + case TransY: + return Frame(Vector(0.0,scale*q+offset,0.0)); + break; + case TransZ: + return Frame(Vector(0.0,0.0,scale*q+offset)); + break; + case Sphere: + // the joint angles represent a rotation vector expressed in the base frame of the joint + // (= the frame you get when there is no offset nor rotation) + return Frame(Rot(Vector((&q)[0], (&q)[1], (&q)[2]))); + break; + case Swing: + // the joint angles represent a 2D rotation vector in the XZ planee of the base frame of the joint + // (= the frame you get when there is no offset nor rotation) + return Frame(Rot(Vector((&q)[0], 0.0, (&q)[1]))); + break; + default: + return Frame::Identity(); + break; + } + } + + Twist Joint::twist(const double& qdot, int dof)const + { + switch(type){ + case RotX: + return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0)); + break; + case RotY: + return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0)); + break; + case RotZ: + return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot)); + break; + case TransX: + return Twist(Vector(scale*qdot,0.0,0.0),Vector(0.0,0.0,0.0)); + break; + case TransY: + return Twist(Vector(0.0,scale*qdot,0.0),Vector(0.0,0.0,0.0)); + break; + case TransZ: + return Twist(Vector(0.0,0.0,scale*qdot),Vector(0.0,0.0,0.0)); + break; + case Swing: + switch (dof) { + case 0: + return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0)); + case 1: + return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot)); + } + return Twist::Zero(); + case Sphere: + switch (dof) { + case 0: + return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0)); + case 1: + return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0)); + case 2: + return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot)); + } + return Twist::Zero(); + default: + return Twist::Zero(); + break; + } + } + + unsigned int Joint::getNDof() const + { + switch (type) { + case Sphere: + return 3; + case Swing: + return 2; + case None: + return 0; + default: + return 1; + } + } + +} // end of namespace KDL + diff --git a/intern/itasc/kdl/joint.hpp b/intern/itasc/kdl/joint.hpp new file mode 100644 index 00000000000..a1291509f0f --- /dev/null +++ b/intern/itasc/kdl/joint.hpp @@ -0,0 +1,138 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_JOINT_HPP +#define KDL_JOINT_HPP + +#include "frames.hpp" +#include <string> + +namespace KDL { + + /** + * \brief This class encapsulates a simple joint, that is with one + * parameterized degree of freedom and with scalar dynamic properties. + * + * A simple joint is described by the following properties : + * - scale: ratio between motion input and motion output + * - offset: between the "physical" and the "logical" zero position. + * - type: revolute or translational, along one of the basic frame axes + * - inertia, stiffness and damping: scalars representing the physical + * effects along/about the joint axis only. + * + * @ingroup KinematicFamily + */ + class Joint { + public: + typedef enum { RotX,RotY,RotZ,TransX,TransY,TransZ,Sphere,Swing,None} JointType; + /** + * Constructor of a joint. + * + * @param type type of the joint, default: Joint::None + * @param scale scale between joint input and actual geometric + * movement, default: 1 + * @param offset offset between joint input and actual + * geometric input, default: 0 + * @param inertia 1D inertia along the joint axis, default: 0 + * @param damping 1D damping along the joint axis, default: 0 + * @param stiffness 1D stiffness along the joint axis, + * default: 0 + */ + Joint(const JointType& type=None,const double& scale=1,const double& offset=0, + const double& inertia=0,const double& damping=0,const double& stiffness=0); + Joint(const Joint& in); + + Joint& operator=(const Joint& arg); + + /** + * Request the 6D-pose between the beginning and the end of + * the joint at joint position q + * + * @param q the 1D joint position + * + * @return the resulting 6D-pose + */ + Frame pose(const double& q)const; + /** + * Request the resulting 6D-velocity with a joint velocity qdot + * + * @param qdot the 1D joint velocity + * + * @return the resulting 6D-velocity + */ + Twist twist(const double& qdot, int dof=0)const; + + /** + * Request the type of the joint. + * + * @return const reference to the type + */ + const JointType& getType() const + { + return type; + }; + + /** + * Request the stringified type of the joint. + * + * @return const string + */ + const std::string getTypeName() const + { + switch (type) { + case RotX: + return "RotX"; + case RotY: + return "RotY"; + case RotZ: + return "RotZ"; + case TransX: + return "TransX"; + case TransY: + return "TransY"; + case TransZ: + return "TransZ"; + case Sphere: + return "Sphere"; + case Swing: + return "Swing"; + case None: + return "None"; + default: + return "None"; + } + }; + unsigned int getNDof() const; + + virtual ~Joint(); + + private: + Joint::JointType type; + double scale; + double offset; + double inertia; + double damping; + double stiffness; + }; + +} // end of namespace KDL + +#endif diff --git a/intern/itasc/kdl/kinfam_io.cpp b/intern/itasc/kdl/kinfam_io.cpp new file mode 100644 index 00000000000..900e2e101a9 --- /dev/null +++ b/intern/itasc/kdl/kinfam_io.cpp @@ -0,0 +1,101 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "kinfam_io.hpp" +#include "frames_io.hpp" + +namespace KDL { +std::ostream& operator <<(std::ostream& os, const Joint& joint) { + return os << joint.getTypeName(); +} + +std::istream& operator >>(std::istream& is, Joint& joint) { + return is; +} + +std::ostream& operator <<(std::ostream& os, const Segment& segment) { + os << "[" << segment.getJoint() << ",\n" << segment.getFrameToTip() << "]"; + return os; +} + +std::istream& operator >>(std::istream& is, Segment& segment) { + return is; +} + +std::ostream& operator <<(std::ostream& os, const Chain& chain) { + os << "["; + for (unsigned int i = 0; i < chain.getNrOfSegments(); i++) + os << chain.getSegment(i) << "\n"; + os << "]"; + return os; +} + +std::istream& operator >>(std::istream& is, Chain& chain) { + return is; +} + +std::ostream& operator <<(std::ostream& os, const Tree& tree) { + SegmentMap::const_iterator root = tree.getSegment("root"); + return os << root; +} + +std::ostream& operator <<(std::ostream& os, SegmentMap::const_iterator root) { + //os<<root->first<<": "<<root->second.segment<<"\n"; + os << root->first<<"(q_nr: "<<root->second.q_nr<<")"<<"\n \t"; + for (unsigned int i = 0; i < root->second.children.size(); i++) { + os <<(root->second.children[i])<<"\t"; + } + return os << "\n"; +} + +std::istream& operator >>(std::istream& is, Tree& tree) { + return is; +} + +std::ostream& operator <<(std::ostream& os, const JntArray& array) { + os << "["; + for (unsigned int i = 0; i < array.rows(); i++) + os << std::setw(KDL_FRAME_WIDTH) << array(i); + os << "]"; + return os; +} + +std::istream& operator >>(std::istream& is, JntArray& array) { + return is; +} + +std::ostream& operator <<(std::ostream& os, const Jacobian& jac) { + os << "["; + for (unsigned int i = 0; i < jac.rows(); i++) { + for (unsigned int j = 0; j < jac.columns(); j++) + os << std::setw(KDL_FRAME_WIDTH) << jac(i, j); + os << std::endl; + } + os << "]"; + return os; +} + +std::istream& operator >>(std::istream& is, Jacobian& jac) { + return is; +} + +} + diff --git a/intern/itasc/kdl/kinfam_io.hpp b/intern/itasc/kdl/kinfam_io.hpp new file mode 100644 index 00000000000..a8dbfd1c5dc --- /dev/null +++ b/intern/itasc/kdl/kinfam_io.hpp @@ -0,0 +1,70 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_KINFAM_IO_HPP +#define KDL_KINFAM_IO_HPP + +#include <iostream> +#include <fstream> + +#include "joint.hpp" +#include "segment.hpp" +#include "chain.hpp" +#include "jntarray.hpp" +#include "jacobian.hpp" +#include "tree.hpp" + +namespace KDL { +std::ostream& operator <<(std::ostream& os, const Joint& joint); +std::istream& operator >>(std::istream& is, Joint& joint); +std::ostream& operator <<(std::ostream& os, const Segment& segment); +std::istream& operator >>(std::istream& is, Segment& segment); +std::ostream& operator <<(std::ostream& os, const Chain& chain); +std::istream& operator >>(std::istream& is, Chain& chain); + +std::ostream& operator <<(std::ostream& os, const Tree& tree); +std::istream& operator >>(std::istream& is, Tree& tree); + +std::ostream& operator <<(std::ostream& os, SegmentMap::const_iterator it); + +std::ostream& operator <<(std::ostream& os, const JntArray& array); +std::istream& operator >>(std::istream& is, JntArray& array); +std::ostream& operator <<(std::ostream& os, const Jacobian& jac); +std::istream& operator >>(std::istream& is, Jacobian& jac); + +template<typename T> +std::ostream& operator<<(std::ostream& os, const std::vector<T>& vec) { + os << "["; + for (unsigned int i = 0; i < vec.size(); i++) + os << vec[i] << " "; + os << "]"; + return os; +} +; + +template<typename T> +std::istream& operator >>(std::istream& is, std::vector<T>& vec) { + return is; +} +; +} +#endif + diff --git a/intern/itasc/kdl/segment.cpp b/intern/itasc/kdl/segment.cpp new file mode 100644 index 00000000000..02f71d5e9f1 --- /dev/null +++ b/intern/itasc/kdl/segment.cpp @@ -0,0 +1,68 @@ +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "segment.hpp" + +namespace KDL { + + Segment::Segment(const Joint& _joint, const Frame& _f_tip, const Inertia& _M): + joint(_joint),M(_M), + f_tip(_f_tip) + { + } + + Segment::Segment(const Segment& in): + joint(in.joint),M(in.M), + f_tip(in.f_tip) + { + } + + Segment& Segment::operator=(const Segment& arg) + { + joint=arg.joint; + M=arg.M; + f_tip=arg.f_tip; + return *this; + } + + Segment::~Segment() + { + } + + Frame Segment::pose(const double& q)const + { + return joint.pose(q)*f_tip; + } + + Twist Segment::twist(const double& q, const double& qdot, int dof)const + { + return joint.twist(qdot, dof).RefPoint(pose(q).p); + } + + Twist Segment::twist(const Vector& p, const double& qdot, int dof)const + { + return joint.twist(qdot, dof).RefPoint(p); + } + + Twist Segment::twist(const Frame& f, const double& qdot, int dof)const + { + return (f.M*joint.twist(qdot, dof)).RefPoint(f.p); + } +}//end of namespace KDL + diff --git a/intern/itasc/kdl/segment.hpp b/intern/itasc/kdl/segment.hpp new file mode 100644 index 00000000000..7c82ab418fa --- /dev/null +++ b/intern/itasc/kdl/segment.hpp @@ -0,0 +1,149 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +#ifndef KDL_SEGMENT_HPP +#define KDL_SEGMENT_HPP + +#include "frames.hpp" +#include "inertia.hpp" +#include "joint.hpp" +#include <vector> + +namespace KDL { + + /** + * \brief This class encapsulates a simple segment, that is a "rigid + * body" (i.e., a frame and an inertia) with a joint and with + * "handles", root and tip to connect to other segments. + * + * A simple segment is described by the following properties : + * - Joint + * - inertia: of the rigid body part of the Segment + * - Offset from the end of the joint to the tip of the segment: + * the joint is located at the root of the segment. + * + * @ingroup KinematicFamily + */ + class Segment { + friend class Chain; + private: + Joint joint; + Inertia M; + Frame f_tip; + + public: + /** + * Constructor of the segment + * + * @param joint joint of the segment, default: + * Joint(Joint::None) + * @param f_tip frame from the end of the joint to the tip of + * the segment, default: Frame::Identity() + * @param M rigid body inertia of the segment, default: Inertia::Zero() + */ + Segment(const Joint& joint=Joint(), const Frame& f_tip=Frame::Identity(),const Inertia& M = Inertia::Zero()); + Segment(const Segment& in); + Segment& operator=(const Segment& arg); + + virtual ~Segment(); + + /** + * Request the pose of the segment, given the joint position q. + * + * @param q 1D position of the joint + * + * @return pose from the root to the tip of the segment + */ + Frame pose(const double& q)const; + /** + * Request the 6D-velocity of the tip of the segment, given + * the joint position q and the joint velocity qdot. + * + * @param q ND position of the joint + * @param qdot ND velocity of the joint + * + * @return 6D-velocity of the tip of the segment, expressed + *in the base-frame of the segment(root) and with the tip of + *the segment as reference point. + */ + Twist twist(const double& q,const double& qdot, int dof=0)const; + + /** + * Request the 6D-velocity at a given point p, relative to base frame of the segment + * givven the joint velocity qdot. + * + * @param p reference point + * @param qdot ND velocity of the joint + * + * @return 6D-velocity at a given point p, expressed + * in the base-frame of the segment(root) + */ + Twist twist(const Vector& p, const double& qdot, int dof=0)const; + + /** + * Request the 6D-velocity at a given frame origin, relative to base frame of the segment + * assuming the frame rotation is the rotation of the joint. + * + * @param f joint pose frame + reference point + * @param qdot ND velocity of the joint + * + * @return 6D-velocity at frame reference point, expressed + * in the base-frame of the segment(root) + */ + Twist twist(const Frame& f, const double& qdot, int dof)const; + + /** + * Request the joint of the segment + * + * + * @return const reference to the joint of the segment + */ + const Joint& getJoint()const + { + return joint; + } + /** + * Request the inertia of the segment + * + * + * @return const reference to the inertia of the segment + */ + const Inertia& getInertia()const + { + return M; + } + + /** + * Request the pose from the joint end to the tip of the + *segment. + * + * @return const reference to the joint end - segment tip pose. + */ + const Frame& getFrameToTip()const + { + return f_tip; + } + + }; +}//end of namespace KDL + +#endif diff --git a/intern/itasc/kdl/tree.cpp b/intern/itasc/kdl/tree.cpp new file mode 100644 index 00000000000..f117e54959b --- /dev/null +++ b/intern/itasc/kdl/tree.cpp @@ -0,0 +1,117 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "tree.hpp" +#include <sstream> +namespace KDL { +using namespace std; + +Tree::Tree() : + nrOfJoints(0), nrOfSegments(0) { + segments.insert(make_pair("root", TreeElement::Root())); +} + +Tree::Tree(const Tree& in) { + segments.clear(); + nrOfSegments = 0; + nrOfJoints = 0; + + segments.insert(make_pair("root", TreeElement::Root())); + this->addTree(in, "", "root"); + +} + +Tree& Tree::operator=(const Tree& in) { + segments.clear(); + nrOfSegments = 0; + nrOfJoints = 0; + + segments.insert(make_pair("root", TreeElement::Root())); + this->addTree(in, "", "root"); + return *this; +} + +bool Tree::addSegment(const Segment& segment, const std::string& segment_name, + const std::string& hook_name) { + SegmentMap::iterator parent = segments.find(hook_name); + //check if parent exists + if (parent == segments.end()) + return false; + pair<SegmentMap::iterator, bool> retval; + //insert new element + retval = segments.insert(make_pair(segment_name, TreeElement(segment, + parent, nrOfJoints))); + //check if insertion succeeded + if (!retval.second) + return false; + //add iterator to new element in parents children list + parent->second.children.push_back(retval.first); + //increase number of segments + nrOfSegments++; + //increase number of joints + nrOfJoints += segment.getJoint().getNDof(); + return true; +} + +bool Tree::addChain(const Chain& chain, const std::string& chain_name, + const std::string& hook_name) { + string parent_name = hook_name; + for (unsigned int i = 0; i < chain.getNrOfSegments(); i++) { + ostringstream segment_name; + segment_name << chain_name << "Segment" << i; + if (this->addSegment(chain.getSegment(i), segment_name.str(), + parent_name)) + parent_name = segment_name.str(); + else + return false; + } + return true; +} + +bool Tree::addTree(const Tree& tree, const std::string& tree_name, + const std::string& hook_name) { + return this->addTreeRecursive(tree.getSegment("root"), tree_name, hook_name); +} + +bool Tree::addTreeRecursive(SegmentMap::const_iterator root, + const std::string& tree_name, const std::string& hook_name) { + //get iterator for root-segment + SegmentMap::const_iterator child; + //try to add all of root's children + for (unsigned int i = 0; i < root->second.children.size(); i++) { + child = root->second.children[i]; + //Try to add the child + if (this->addSegment(child->second.segment, tree_name + child->first, + hook_name)) { + //if child is added, add all the child's children + if (!(this->addTreeRecursive(child, tree_name, tree_name + + child->first))) + //if it didn't work, return false + return false; + } else + //If the child could not be added, return false + return false; + } + return true; +} + +} + diff --git a/intern/itasc/kdl/tree.hpp b/intern/itasc/kdl/tree.hpp new file mode 100644 index 00000000000..bdd3aa94572 --- /dev/null +++ b/intern/itasc/kdl/tree.hpp @@ -0,0 +1,167 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_TREE_HPP +#define KDL_TREE_HPP + +#include "segment.hpp" +#include "chain.hpp" + +#include <string> +#include <map> + +namespace KDL +{ + //Forward declaration + class TreeElement; + typedef std::map<std::string,TreeElement> SegmentMap; + + class TreeElement + { + private: + TreeElement():q_nr(0) + {}; + public: + Segment segment; + unsigned int q_nr; + SegmentMap::const_iterator parent; + std::vector<SegmentMap::const_iterator > children; + TreeElement(const Segment& segment_in,const SegmentMap::const_iterator& parent_in,unsigned int q_nr_in) + { + q_nr=q_nr_in; + segment=segment_in; + parent=parent_in; + }; + static TreeElement Root() + { + return TreeElement(); + }; + }; + + /** + * \brief This class encapsulates a <strong>tree</strong> + * kinematic interconnection structure. It is build out of segments. + * + * @ingroup KinematicFamily + */ + class Tree + { + private: + SegmentMap segments; + unsigned int nrOfJoints; + unsigned int nrOfSegments; + + bool addTreeRecursive(SegmentMap::const_iterator root, const std::string& tree_name, const std::string& hook_name); + + public: + /** + * The constructor of a tree, a new tree is always empty + */ + Tree(); + Tree(const Tree& in); + Tree& operator= (const Tree& arg); + + /** + * Adds a new segment to the end of the segment with + * hook_name as segment_name + * + * @param segment new segment to add + * @param segment_name name of the new segment + * @param hook_name name of the segment to connect this + * segment with. + * + * @return false if hook_name could not be found. + */ + bool addSegment(const Segment& segment, const std::string& segment_name, const std::string& hook_name); + + /** + * Adds a complete chain to the end of the segment with + * hook_name as segment_name. Segment i of + * the chain will get chain_name+".Segment"+i as segment_name. + * + * @param chain Chain to add + * @param chain_name name of the chain + * @param hook_name name of the segment to connect the chain with. + * + * @return false if hook_name could not be found. + */ + bool addChain(const Chain& chain, const std::string& chain_name, const std::string& hook_name); + + /** + * Adds a complete tree to the end of the segment with + * hookname as segment_name. The segments of the tree will get + * tree_name+segment_name as segment_name. + * + * @param tree Tree to add + * @param tree_name name of the tree + * @param hook_name name of the segment to connect the tree with + * + * @return false if hook_name could not be found + */ + bool addTree(const Tree& tree, const std::string& tree_name,const std::string& hook_name); + + /** + * Request the total number of joints in the tree.\n + * <strong> Important:</strong> It is not the same as the + * total number of segments since a segment does not need to have + * a joint. + * + * @return total nr of joints + */ + unsigned int getNrOfJoints()const + { + return nrOfJoints; + }; + + /** + * Request the total number of segments in the tree. + * @return total number of segments + */ + unsigned int getNrOfSegments()const {return nrOfSegments;}; + + /** + * Request the segment of the tree with name segment_name. + * + * @param segment_name the name of the requested segment + * + * @return constant iterator pointing to the requested segment + */ + SegmentMap::const_iterator getSegment(const std::string& segment_name)const + { + return segments.find(segment_name); + }; + + + + const SegmentMap& getSegments()const + { + return segments; + } + + virtual ~Tree(){}; + }; +} +#endif + + + + + diff --git a/intern/itasc/kdl/treefksolver.hpp b/intern/itasc/kdl/treefksolver.hpp new file mode 100644 index 00000000000..22d5400ab0a --- /dev/null +++ b/intern/itasc/kdl/treefksolver.hpp @@ -0,0 +1,110 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Copyright (C) 2008 Julia Jesse + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_TREE_FKSOLVER_HPP +#define KDL_TREE_FKSOLVER_HPP + +#include <string> + +#include "tree.hpp" +//#include "framevel.hpp" +//#include "frameacc.hpp" +#include "jntarray.hpp" +//#include "jntarrayvel.hpp" +//#include "jntarrayacc.hpp" + +namespace KDL { + + /** + * \brief This <strong>abstract</strong> class encapsulates a + * solver for the forward position kinematics for a KDL::Tree. + * + * @ingroup KinematicFamily + */ + + //Forward definition + class TreeFkSolverPos { + public: + /** + * Calculate forward position kinematics for a KDL::Tree, + * from joint coordinates to cartesian pose. + * + * @param q_in input joint coordinates + * @param p_out reference to output cartesian pose + * + * @return if < 0 something went wrong + */ + virtual int JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName)=0; + virtual ~TreeFkSolverPos(){}; + }; + + /** + * \brief This <strong>abstract</strong> class encapsulates a solver + * for the forward velocity kinematics for a KDL::Tree. + * + * @ingroup KinematicFamily + */ +// class TreeFkSolverVel { +// public: + /** + * Calculate forward position and velocity kinematics, from + * joint coordinates to cartesian coordinates. + * + * @param q_in input joint coordinates (position and velocity) + * @param out output cartesian coordinates (position and velocity) + * + * @return if < 0 something went wrong + */ +// virtual int JntToCart(const JntArrayVel& q_in, FrameVel& out,int segmentNr=-1)=0; + +// virtual ~TreeFkSolverVel(){}; +// }; + + /** + * \brief This <strong>abstract</strong> class encapsulates a solver + * for the forward acceleration kinematics for a KDL::Tree. + * + * @ingroup KinematicFamily + */ + +// class TreeFkSolverAcc { +// public: + /** + * Calculate forward position, velocity and accelaration + * kinematics, from joint coordinates to cartesian coordinates + * + * @param q_in input joint coordinates (position, velocity and + * acceleration + @param out output cartesian coordinates (position, velocity + * and acceleration + * + * @return if < 0 something went wrong + */ +// virtual int JntToCart(const JntArrayAcc& q_in, FrameAcc& out,int segmentNr=-1)=0; + +// virtual ~TreeFkSolverAcc()=0; +// }; + + +}//end of namespace KDL + +#endif diff --git a/intern/itasc/kdl/treefksolverpos_recursive.cpp b/intern/itasc/kdl/treefksolverpos_recursive.cpp new file mode 100644 index 00000000000..f9dcb336d5d --- /dev/null +++ b/intern/itasc/kdl/treefksolverpos_recursive.cpp @@ -0,0 +1,70 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Copyright (C) 2008 Julia Jesse + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "treefksolverpos_recursive.hpp" +#include <iostream> + +namespace KDL { + + TreeFkSolverPos_recursive::TreeFkSolverPos_recursive(const Tree& _tree): + tree(_tree) + { + } + + int TreeFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName) + { + SegmentMap::const_iterator it = tree.getSegment(segmentName); + SegmentMap::const_iterator baseit = tree.getSegment(baseName); + + if(q_in.rows() != tree.getNrOfJoints()) + return -1; + else if(it == tree.getSegments().end()) //if the segment name is not found + return -2; + else if(baseit == tree.getSegments().end()) //if the base segment name is not found + return -3; + else{ + p_out = recursiveFk(q_in, it, baseit); + return 0; + } + } + + Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it, const SegmentMap::const_iterator& baseit) + { + //gets the frame for the current element (segment) + const TreeElement& currentElement = it->second; + + if(it == baseit){ + return KDL::Frame::Identity(); + } + else{ + Frame currentFrame = currentElement.segment.pose(((JntArray&)q_in)(currentElement.q_nr)); + SegmentMap::const_iterator parentIt = currentElement.parent; + return recursiveFk(q_in, parentIt, baseit) * currentFrame; + } + } + + TreeFkSolverPos_recursive::~TreeFkSolverPos_recursive() + { + } + + +} diff --git a/intern/itasc/kdl/treefksolverpos_recursive.hpp b/intern/itasc/kdl/treefksolverpos_recursive.hpp new file mode 100644 index 00000000000..c22fe4af75b --- /dev/null +++ b/intern/itasc/kdl/treefksolverpos_recursive.hpp @@ -0,0 +1,53 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Copyright (C) 2008 Julia Jesse + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDLTREEFKSOLVERPOS_RECURSIVE_HPP +#define KDLTREEFKSOLVERPOS_RECURSIVE_HPP + +#include "treefksolver.hpp" + +namespace KDL { + + /** + * Implementation of a recursive forward position kinematics + * algorithm to calculate the position transformation from joint + * space to Cartesian space of a general kinematic tree (KDL::Tree). + * + * @ingroup KinematicFamily + */ + class TreeFkSolverPos_recursive : public TreeFkSolverPos + { + public: + TreeFkSolverPos_recursive(const Tree& tree); + ~TreeFkSolverPos_recursive(); + + virtual int JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName); + + private: + const Tree tree; + + Frame recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it, const SegmentMap::const_iterator& baseit); + }; + +} + +#endif diff --git a/intern/itasc/kdl/treejnttojacsolver.cpp b/intern/itasc/kdl/treejnttojacsolver.cpp new file mode 100644 index 00000000000..194f18eb959 --- /dev/null +++ b/intern/itasc/kdl/treejnttojacsolver.cpp @@ -0,0 +1,78 @@ +/* + * TreeJntToJacSolver.cpp + * + * Created on: Nov 27, 2008 + * Author: rubensmits + */ + +#include "treejnttojacsolver.hpp" +#include <iostream> + +namespace KDL { + +TreeJntToJacSolver::TreeJntToJacSolver(const Tree& tree_in) : + tree(tree_in) { +} + +TreeJntToJacSolver::~TreeJntToJacSolver() { +} + +int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, + const std::string& segmentname) { + //First we check all the sizes: + if (q_in.rows() != tree.getNrOfJoints() || jac.columns() + != tree.getNrOfJoints()) + return -1; + + //Lets search the tree-element + SegmentMap::const_iterator it = tree.getSegments().find(segmentname); + + //If segmentname is not inside the tree, back out: + if (it == tree.getSegments().end()) + return -2; + + //Let's make the jacobian zero: + SetToZero(jac); + + SegmentMap::const_iterator root = tree.getSegments().find("root"); + + Frame T_total = Frame::Identity(); + Frame T_local, T_joint; + Twist t_local; + //Lets recursively iterate until we are in the root segment + while (it != root) { + //get the corresponding q_nr for this TreeElement: + unsigned int q_nr = it->second.q_nr; + + //get the pose of the joint. + T_joint = it->second.segment.getJoint().pose(((JntArray&)q_in)(q_nr)); + // combine with the tip to have the tip pose + T_local = T_joint*it->second.segment.getFrameToTip(); + //calculate new T_end: + T_total = T_local * T_total; + + //get the twist of the segment: + int ndof = it->second.segment.getJoint().getNDof(); + for (int dof=0; dof<ndof; dof++) { + // combine joint rotation with tip position to get a reference frame for the joint + T_joint.p = T_local.p; + // in which the twist can be computed (needed for NDof joint) + t_local = it->second.segment.twist(T_joint, 1.0, dof); + //transform the endpoint of the local twist to the global endpoint: + t_local = t_local.RefPoint(T_total.p - T_local.p); + //transform the base of the twist to the endpoint + t_local = T_total.M.Inverse(t_local); + //store the twist in the jacobian: + jac.twists[q_nr+dof] = t_local; + } + //goto the parent + it = it->second.parent; + }//endwhile + //Change the base of the complete jacobian from the endpoint to the base + changeBase(jac, T_total.M, jac); + + return 0; + +}//end JntToJac +}//end namespace + diff --git a/intern/itasc/kdl/treejnttojacsolver.hpp b/intern/itasc/kdl/treejnttojacsolver.hpp new file mode 100644 index 00000000000..40977dcd577 --- /dev/null +++ b/intern/itasc/kdl/treejnttojacsolver.hpp @@ -0,0 +1,38 @@ +/* + * TreeJntToJacSolver.hpp + * + * Created on: Nov 27, 2008 + * Author: rubensmits + */ + +#ifndef TREEJNTTOJACSOLVER_HPP_ +#define TREEJNTTOJACSOLVER_HPP_ + +#include "tree.hpp" +#include "jacobian.hpp" +#include "jntarray.hpp" + +namespace KDL { + +class TreeJntToJacSolver { +public: + TreeJntToJacSolver(const Tree& tree); + + virtual ~TreeJntToJacSolver(); + + /* + * Calculate the jacobian for a part of the tree: from a certain segment, given by segmentname to the root. + * The resulting jacobian is expressed in the baseframe of the tree ("root"), the reference point is in the end-segment + */ + + int JntToJac(const JntArray& q_in, Jacobian& jac, + const std::string& segmentname); + +private: + KDL::Tree tree; + +}; + +}//End of namespace + +#endif /* TREEJNTTOJACSOLVER_H_ */ diff --git a/intern/itasc/kdl/utilities/Makefile b/intern/itasc/kdl/utilities/Makefile new file mode 100644 index 00000000000..8ee08089e10 --- /dev/null +++ b/intern/itasc/kdl/utilities/Makefile @@ -0,0 +1,40 @@ +# +# $Id$ +# +# ***** BEGIN GPL LICENSE BLOCK ***** +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software Foundation, +# Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +# +# The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. +# All rights reserved. +# +# The Original Code is: all of this file. +# +# Contributor(s): Hans Lambermont +# +# ***** END GPL LICENSE BLOCK ***** +# iksolver main makefile. +# + +include nan_definitions.mk + +LIBNAME = itasc_kdl_util +# Same dir than parent (itasc instead of $(LIBNAME)) +DIR = $(OCGDIR)/intern/itasc + +include nan_compile.mk + +CPPFLAGS += -I. +CPPFLAGS += -I../../../../extern/Eigen2 diff --git a/intern/itasc/kdl/utilities/error.h b/intern/itasc/kdl/utilities/error.h new file mode 100644 index 00000000000..868daef3db3 --- /dev/null +++ b/intern/itasc/kdl/utilities/error.h @@ -0,0 +1,245 @@ +/*************************************************************************** + tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 error.h + + error.h - description + ------------------- + begin : Mon January 10 2005 + copyright : (C) 2005 Erwin Aertbelien + email : erwin.aertbelien@mech.kuleuven.ac.be + + *************************************************************************** + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Lesser General Public * + * License as published by the Free Software Foundation; either * + * version 2.1 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with this library; if not, write to the Free Software * + * Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307 USA * + * * + ***************************************************************************/ + + +/***************************************************************************** + * \file + * Defines the exception classes that can be thrown + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id$ + * $Name: $ + ****************************************************************************/ +#ifndef ERROR_H_84822 // to make it unique, a random number +#define ERROR_H_84822 + +#include "utility.h" +#include <string> + +namespace KDL { + +/** + * Base class for errors generated by ORO_Geometry + */ +class Error { +public: + /** Returns a description string describing the error. + * the returned pointer only garanteed to exists as long as + * the Error object exists. + */ + virtual ~Error() {} + virtual const char* Description() const {return "Unspecified Error\n";} + + virtual int GetType() const {return 0;} +}; + + +class Error_IO : public Error { + std::string msg; + int typenr; +public: + Error_IO(const std::string& _msg="Unspecified I/O Error",int typenr=0):msg(_msg) {} + virtual const char* Description() const {return msg.c_str();} + virtual int GetType() const {return typenr;} +}; +class Error_BasicIO : public Error_IO {}; +class Error_BasicIO_File : public Error_BasicIO { +public: + virtual const char* Description() const {return "Error while reading stream";} + virtual int GetType() const {return 1;} +}; +class Error_BasicIO_Exp_Delim : public Error_BasicIO { +public: + virtual const char* Description() const {return "Expected Delimiter not encountered";} + virtual int GetType() const {return 2;} +}; +class Error_BasicIO_Not_A_Space : public Error_BasicIO { +public: + virtual const char* Description() const {return "Expected space,tab or newline not encountered";} + virtual int GetType() const {return 3;} +}; +class Error_BasicIO_Unexpected : public Error_BasicIO { +public: + virtual const char* Description() const {return "Unexpected character";} + virtual int GetType() const {return 4;} +}; + +class Error_BasicIO_ToBig : public Error_BasicIO { +public: + virtual const char* Description() const {return "Word that is read out of stream is bigger than maxsize";} + virtual int GetType() const {return 5;} +}; + +class Error_BasicIO_Not_Opened : public Error_BasicIO { +public: + virtual const char* Description() const {return "File cannot be opened";} + virtual int GetType() const {return 6;} +}; +class Error_FrameIO : public Error_IO {}; +class Error_Frame_Vector_Unexpected_id : public Error_FrameIO { +public: + virtual const char* Description() const {return "Unexpected identifier, expecting a vector (explicit or ZERO)";} + virtual int GetType() const {return 101;} +}; +class Error_Frame_Frame_Unexpected_id : public Error_FrameIO { +public: + virtual const char* Description() const {return "Unexpected identifier, expecting a Frame (explicit or DH)";} + virtual int GetType() const {return 102;} +}; +class Error_Frame_Rotation_Unexpected_id : public Error_FrameIO { +public: + virtual const char* Description() const {return "Unexpected identifier, expecting a Rotation (explicit or EULERZYX, EULERZYZ, RPY,ROT,IDENTITY)";} + virtual int GetType() const {return 103;} +}; +class Error_ChainIO : public Error {}; +class Error_Chain_Unexpected_id : public Error_ChainIO { +public: + virtual const char* Description() const {return "Unexpected identifier, expecting TRANS or ROT";} + virtual int GetType() const {return 201;} +}; +//! Error_Redundancy indicates an error that occured during solving for redundancy. +class Error_RedundancyIO:public Error_IO {}; +class Error_Redundancy_Illegal_Resolutiontype : public Error_RedundancyIO { +public: + virtual const char* Description() const {return "Illegal Resolutiontype is used in I/O with ResolutionTask";} + virtual int GetType() const {return 301;} +}; +class Error_Redundancy:public Error {}; +class Error_Redundancy_Unavoidable : public Error_Redundancy { +public: + virtual const char* Description() const {return "Joint limits cannot be avoided";} + virtual int GetType() const {return 1002;} +}; +class Error_Redundancy_Low_Manip: public Error_Redundancy { +public: + virtual const char* Description() const {return "Manipulability is very low";} + virtual int GetType() const {return 1003;} +}; +class Error_MotionIO : public Error {}; +class Error_MotionIO_Unexpected_MotProf : public Error_MotionIO { +public: + virtual const char* Description() const { return "Wrong keyword while reading motion profile";} + virtual int GetType() const {return 2001;} +}; +class Error_MotionIO_Unexpected_Traj : public Error_MotionIO { +public: + virtual const char* Description() const { return "Trajectory type keyword not known";} + virtual int GetType() const {return 2002;} +}; + +class Error_MotionPlanning : public Error {}; + +class Error_MotionPlanning_Circle_ToSmall : public Error_MotionPlanning { +public: + virtual const char* Description() const { return "Circle : radius is to small";} + virtual int GetType() const {return 3001;} +}; + +class Error_MotionPlanning_Circle_No_Plane : public Error_MotionPlanning { +public: + virtual const char* Description() const { return "Circle : Plane for motion is not properly defined";} + virtual int GetType() const {return 3002;} +}; + +class Error_MotionPlanning_Incompatible: public Error_MotionPlanning { +public: + virtual const char* Description() const { return "Acceleration of a rectangular velocityprofile cannot be used";} + virtual int GetType() const {return 3003;} +}; + +class Error_MotionPlanning_Not_Feasible: public Error_MotionPlanning { +public: + virtual const char* Description() const { return "Motion Profile with requested parameters is not feasible";} + virtual int GetType() const {return 3004;} +}; + +class Error_MotionPlanning_Not_Applicable: public Error_MotionPlanning { +public: + virtual const char* Description() const { return "Method is not applicable for this derived object";} + virtual int GetType() const {return 3004;} +}; +//! Abstract subclass of all errors that can be thrown by Adaptive_Integrator +class Error_Integrator : public Error {}; + +//! Error_Stepsize_Underflow is thrown if the stepsize becomes to small +class Error_Stepsize_Underflow : public Error_Integrator { +public: + virtual const char* Description() const { return "Stepsize Underflow";} + virtual int GetType() const {return 4001;} +}; + +//! Error_To_Many_Steps is thrown if the number of steps needed to +//! integrate to the desired accuracy becomes to big. +class Error_To_Many_Steps : public Error_Integrator { +public: + virtual const char* Description() const { return "To many steps"; } + virtual int GetType() const {return 4002;} +}; + +//! Error_Stepsize_To_Small is thrown if the stepsize becomes to small +class Error_Stepsize_To_Small : public Error_Integrator { +public: + virtual const char* Description() const { return "Stepsize to small"; } + virtual int GetType() const {return 4003;} +}; + +class Error_Criterium : public Error {}; + +class Error_Criterium_Unexpected_id: public Error_Criterium { +public: + virtual const char* Description() const { return "Unexpected identifier while reading a criterium"; } + virtual int GetType() const {return 5001;} +}; + +class Error_Limits : public Error {}; + +class Error_Limits_Unexpected_id: public Error_Limits { +public: + virtual const char* Description() const { return "Unexpected identifier while reading a jointlimits"; } + virtual int GetType() const {return 6001;} +}; + + +class Error_Not_Implemented: public Error { +public: + virtual const char* Description() const { return "The requested object/method/function is not implemented"; } + virtual int GetType() const {return 7000;} +}; + + + +} + +#endif diff --git a/intern/itasc/kdl/utilities/error_stack.cpp b/intern/itasc/kdl/utilities/error_stack.cpp new file mode 100644 index 00000000000..d55308c7346 --- /dev/null +++ b/intern/itasc/kdl/utilities/error_stack.cpp @@ -0,0 +1,59 @@ +/***************************************************************************** + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id$ + * $Name: $ + ****************************************************************************/ + + +#include "error_stack.h" +#include <stack> +#include <vector> +#include <string> +#include <cstring> + +namespace KDL { + +// Trace of the call stack of the I/O routines to help user +// interprete error messages from I/O +typedef std::stack<std::string> ErrorStack; + +ErrorStack errorstack; +// should be in Thread Local Storage if this gets multithreaded one day... + + +void IOTrace(const std::string& description) { + errorstack.push(description); +} + + +void IOTracePop() { + errorstack.pop(); +} + +void IOTraceOutput(std::ostream& os) { + while (!errorstack.empty()) { + os << errorstack.top().c_str() << std::endl; + errorstack.pop(); + } +} + + +void IOTracePopStr(char* buffer,int size) { + if (errorstack.empty()) { + *buffer = 0; + return; + } + strncpy(buffer,errorstack.top().c_str(),size); + errorstack.pop(); +} + +} diff --git a/intern/itasc/kdl/utilities/error_stack.h b/intern/itasc/kdl/utilities/error_stack.h new file mode 100644 index 00000000000..918bc0786a6 --- /dev/null +++ b/intern/itasc/kdl/utilities/error_stack.h @@ -0,0 +1,70 @@ +/*************************************************************************** + tag: Erwin Aertbelien Mon Jan 10 16:38:39 CET 2005 error_stack.h + + error_stack.h - description + ------------------- + begin : Mon January 10 2005 + copyright : (C) 2005 Erwin Aertbelien + email : erwin.aertbelien@mech.kuleuven.ac.be + + *************************************************************************** + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Lesser General Public * + * License as published by the Free Software Foundation; either * + * version 2.1 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with this library; if not, write to the Free Software * + * Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307 USA * + * * + ***************************************************************************/ + + +/** + * \file + * \author Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * \version + * ORO_Geometry V0.2 + * + * \par history + * - changed layout of the comments to accomodate doxygen + */ +#ifndef ERROR_STACK_H +#define ERROR_STACK_H + +#include "utility.h" +#include "utility_io.h" +#include <string> + + +namespace KDL { + +/* + * \todo + * IOTrace-routines store in static memory, should be in thread-local memory. + * pushes a description of the current routine on the IO-stack trace + */ +void IOTrace(const std::string& description); + +//! pops a description of the IO-stack +void IOTracePop(); + + +//! outputs the IO-stack to a stream to provide a better errormessage. +void IOTraceOutput(std::ostream& os); + +//! outputs one element of the IO-stack to the buffer (maximally size chars) +//! returns empty string if no elements on the stack. +void IOTracePopStr(char* buffer,int size); + + +} + +#endif + diff --git a/intern/itasc/kdl/utilities/kdl-config.h b/intern/itasc/kdl/utilities/kdl-config.h new file mode 100644 index 00000000000..4d2df2df6c5 --- /dev/null +++ b/intern/itasc/kdl/utilities/kdl-config.h @@ -0,0 +1,33 @@ +/* Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> */ + +/* Version: 1.0 */ +/* Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> */ +/* Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> */ +/* URL: http://www.orocos.org/kdl */ + +/* This library is free software; you can redistribute it and/or */ +/* modify it under the terms of the GNU Lesser General Public */ +/* License as published by the Free Software Foundation; either */ +/* version 2.1 of the License, or (at your option) any later version. */ + +/* This library is distributed in the hope that it will be useful, */ +/* but WITHOUT ANY WARRANTY; without even the implied warranty of */ +/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU */ +/* Lesser General Public License for more details. */ + +/* You should have received a copy of the GNU Lesser General Public */ +/* License along with this library; if not, write to the Free Software */ +/* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA */ + + +/* Methods are inlined */ +#define KDL_INLINE 1 + +/* Column width that is used form printing frames */ +#define KDL_FRAME_WIDTH 12 + +/* Indices are checked when accessing members of the objects */ +#define KDL_INDEX_CHECK 1 + +/* use KDL implementation for == operator */ +#define KDL_USE_EQUAL 1 diff --git a/intern/itasc/kdl/utilities/rall1d.h b/intern/itasc/kdl/utilities/rall1d.h new file mode 100644 index 00000000000..98bd4385d1e --- /dev/null +++ b/intern/itasc/kdl/utilities/rall1d.h @@ -0,0 +1,478 @@ + +/***************************************************************************** + * \file + * class for automatic differentiation on scalar values and 1st + * derivatives . + * + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par Note + * VC6++ contains a bug, concerning the use of inlined friend functions + * in combination with namespaces. So, try to avoid inlined friend + * functions ! + * + * \par History + * - $log$ + * + * \par Release + * $Id$ + * $Name: $ + ****************************************************************************/ + +#ifndef Rall1D_H +#define Rall1D_H +#include <assert.h> +#include "utility.h" + +namespace KDL { +/** + * Rall1d contains a value, and its gradient, and defines an algebraic structure on this pair. + * This template class has 3 template parameters : + * - T contains the type of the value. + * - V contains the type of the gradient (can be a vector-like type). + * - S defines a scalar type that can operate on Rall1d. This is the type that + * is used to give back values of Norm() etc. + * + * S is usefull when you recurse a Rall1d object into itself to create a 2nd, 3th, 4th,.. + * derivatives. (e.g. Rall1d< Rall1d<double>, Rall1d<double>, double> ). + * + * S is always passed by value. + * + * \par Class Type + * Concrete implementation + */ +template <typename T,typename V=T,typename S=T> +class Rall1d + { + public: + typedef T valuetype; + typedef V gradienttype; + typedef S scalartype; + public : + T t; //!< value + V grad; //!< gradient + public : + INLINE Rall1d() {} + + T value() const { + return t; + } + V deriv() const { + return grad; + } + + explicit INLINE Rall1d(typename TI<T>::Arg c) + {t=T(c);SetToZero(grad);} + + INLINE Rall1d(typename TI<T>::Arg tn, typename TI<V>::Arg afg):t(tn),grad(afg) {} + + INLINE Rall1d(const Rall1d<T,V,S>& r):t(r.t),grad(r.grad) {} + //if one defines this constructor, it's better optimized then the + //automatically generated one ( this one set's up a loop to copy + // word by word. + + INLINE T& Value() { + return t; + } + + INLINE V& Gradient() { + return grad; + } + + INLINE static Rall1d<T,V,S> Zero() { + Rall1d<T,V,S> tmp; + SetToZero(tmp); + return tmp; + } + INLINE static Rall1d<T,V,S> Identity() { + Rall1d<T,V,S> tmp; + SetToIdentity(tmp); + return tmp; + } + + INLINE Rall1d<T,V,S>& operator =(S c) + {t=c;SetToZero(grad);return *this;} + + INLINE Rall1d<T,V,S>& operator =(const Rall1d<T,V,S>& r) + {t=r.t;grad=r.grad;return *this;} + + INLINE Rall1d<T,V,S>& operator /=(const Rall1d<T,V,S>& rhs) + { + grad = LinComb(rhs.t,grad,-t,rhs.grad) / (rhs.t*rhs.t); + t /= rhs.t; + return *this; + } + + INLINE Rall1d<T,V,S>& operator *=(const Rall1d<T,V,S>& rhs) + { + LinCombR(rhs.t,grad,t,rhs.grad,grad); + t *= rhs.t; + return *this; + } + + INLINE Rall1d<T,V,S>& operator +=(const Rall1d<T,V,S>& rhs) + { + grad +=rhs.grad; + t +=rhs.t; + return *this; + } + + INLINE Rall1d<T,V,S>& operator -=(const Rall1d<T,V,S>& rhs) + { + grad -= rhs.grad; + t -= rhs.t; + return *this; + } + + INLINE Rall1d<T,V,S>& operator /=(S rhs) + { + grad /= rhs; + t /= rhs; + return *this; + } + + INLINE Rall1d<T,V,S>& operator *=(S rhs) + { + grad *= rhs; + t *= rhs; + return *this; + } + + INLINE Rall1d<T,V,S>& operator +=(S rhs) + { + t += rhs; + return *this; + } + + INLINE Rall1d<T,V,S>& operator -=(S rhs) + { + t -= rhs; + return *this; + } + + + + // = operators + /* gives warnings on cygwin + + template <class T2,class V2,class S2> + friend INLINE Rall1d<T2,V2,S2> operator /(const Rall1d<T2,V2,S2>& lhs,const Rall1d<T2,V2,S2>& rhs); + + friend INLINE Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs); + friend INLINE Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs); + friend INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs); + friend INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& arg); + friend INLINE Rall1d<T,V,S> operator *(S s,const Rall1d<T,V,S>& v); + friend INLINE Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& v,S s); + friend INLINE Rall1d<T,V,S> operator +(S s,const Rall1d<T,V,S>& v); + friend INLINE Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& v,S s); + friend INLINE Rall1d<T,V,S> operator -(S s,const Rall1d<T,V,S>& v); + friend INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& v,S s); + friend INLINE Rall1d<T,V,S> operator /(S s,const Rall1d<T,V,S>& v); + friend INLINE Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& v,S s); + + // = Mathematical functions that operate on Rall1d objects + friend INLINE Rall1d<T,V,S> exp(const Rall1d<T,V,S>& arg); + friend INLINE Rall1d<T,V,S> log(const Rall1d<T,V,S>& arg); + friend INLINE Rall1d<T,V,S> sin(const Rall1d<T,V,S>& arg); + friend INLINE Rall1d<T,V,S> cos(const Rall1d<T,V,S>& arg); + friend INLINE Rall1d<T,V,S> tan(const Rall1d<T,V,S>& arg); + friend INLINE Rall1d<T,V,S> sinh(const Rall1d<T,V,S>& arg); + friend INLINE Rall1d<T,V,S> cosh(const Rall1d<T,V,S>& arg); + friend INLINE Rall1d<T,V,S> sqr(const Rall1d<T,V,S>& arg); + friend INLINE Rall1d<T,V,S> pow(const Rall1d<T,V,S>& arg,double m) ; + friend INLINE Rall1d<T,V,S> sqrt(const Rall1d<T,V,S>& arg); + friend INLINE Rall1d<T,V,S> atan(const Rall1d<T,V,S>& x); + friend INLINE Rall1d<T,V,S> hypot(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x); + friend INLINE Rall1d<T,V,S> asin(const Rall1d<T,V,S>& x); + friend INLINE Rall1d<T,V,S> acos(const Rall1d<T,V,S>& x); + friend INLINE Rall1d<T,V,S> abs(const Rall1d<T,V,S>& x); + friend INLINE S Norm(const Rall1d<T,V,S>& value) ; + friend INLINE Rall1d<T,V,S> tanh(const Rall1d<T,V,S>& arg); + friend INLINE Rall1d<T,V,S> atan2(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x); + + // = Utility functions to improve performance + + friend INLINE Rall1d<T,V,S> LinComb(S alfa,const Rall1d<T,V,S>& a, + const T& beta,const Rall1d<T,V,S>& b ); + + friend INLINE void LinCombR(S alfa,const Rall1d<T,V,S>& a, + const T& beta,const Rall1d<T,V,S>& b,Rall1d<T,V,S>& result ); + + // = Setting value of a Rall1d object to 0 or 1 + + friend INLINE void SetToZero(Rall1d<T,V,S>& value); + friend INLINE void SetToOne(Rall1d<T,V,S>& value); + // = Equality in an eps-interval + friend INLINE bool Equal(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x,double eps); + */ + }; + + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs) + { + return Rall1d<T,V,S>(lhs.t/rhs.t,(lhs.grad*rhs.t-lhs.t*rhs.grad)/(rhs.t*rhs.t)); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs) + { + return Rall1d<T,V,S>(lhs.t*rhs.t,rhs.t*lhs.grad+lhs.t*rhs.grad); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs) + { + return Rall1d<T,V,S>(lhs.t+rhs.t,lhs.grad+rhs.grad); + } + + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs) + { + return Rall1d<T,V,S>(lhs.t-rhs.t,lhs.grad-rhs.grad); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& arg) + { + return Rall1d<T,V,S>(-arg.t,-arg.grad); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> operator *(S s,const Rall1d<T,V,S>& v) + { + return Rall1d<T,V,S>(s*v.t,s*v.grad); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& v,S s) + { + return Rall1d<T,V,S>(v.t*s,v.grad*s); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> operator +(S s,const Rall1d<T,V,S>& v) + { + return Rall1d<T,V,S>(s+v.t,v.grad); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& v,S s) + { + return Rall1d<T,V,S>(v.t+s,v.grad); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> operator -(S s,const Rall1d<T,V,S>& v) + { + return Rall1d<T,V,S>(s-v.t,-v.grad); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& v,S s) + { + return Rall1d<T,V,S>(v.t-s,v.grad); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> operator /(S s,const Rall1d<T,V,S>& v) + { + return Rall1d<T,V,S>(s/v.t,(-s*v.grad)/(v.t*v.t)); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& v,S s) + { + return Rall1d<T,V,S>(v.t/s,v.grad/s); + } + + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> exp(const Rall1d<T,V,S>& arg) + { + T v; + v= (exp(arg.t)); + return Rall1d<T,V,S>(v,v*arg.grad); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> log(const Rall1d<T,V,S>& arg) + { + T v; + v=(log(arg.t)); + return Rall1d<T,V,S>(v,arg.grad/arg.t); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> sin(const Rall1d<T,V,S>& arg) + { + T v; + v=(sin(arg.t)); + return Rall1d<T,V,S>(v,cos(arg.t)*arg.grad); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> cos(const Rall1d<T,V,S>& arg) + { + T v; + v=(cos(arg.t)); + return Rall1d<T,V,S>(v,-sin(arg.t)*arg.grad); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> tan(const Rall1d<T,V,S>& arg) + { + T v; + v=(tan(arg.t)); + return Rall1d<T,V,S>(v,arg.grad/sqr(cos(arg.t))); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> sinh(const Rall1d<T,V,S>& arg) + { + T v; + v=(sinh(arg.t)); + return Rall1d<T,V,S>(v,cosh(arg.t)*arg.grad); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> cosh(const Rall1d<T,V,S>& arg) + { + T v; + v=(cosh(arg.t)); + return Rall1d<T,V,S>(v,sinh(arg.t)*arg.grad); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> sqr(const Rall1d<T,V,S>& arg) + { + T v; + v=(arg.t*arg.t); + return Rall1d<T,V,S>(v,(2.0*arg.t)*arg.grad); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> pow(const Rall1d<T,V,S>& arg,double m) + { + T v; + v=(pow(arg.t,m)); + return Rall1d<T,V,S>(v,(m*v/arg.t)*arg.grad); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> sqrt(const Rall1d<T,V,S>& arg) + { + T v; + v=sqrt(arg.t); + return Rall1d<T,V,S>(v, (0.5/v)*arg.grad); + } + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> atan(const Rall1d<T,V,S>& x) +{ + T v; + v=(atan(x.t)); + return Rall1d<T,V,S>(v,x.grad/(1.0+sqr(x.t))); +} + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> hypot(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x) +{ + T v; + v=(hypot(y.t,x.t)); + return Rall1d<T,V,S>(v,(x.t/v)*x.grad+(y.t/v)*y.grad); +} + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> asin(const Rall1d<T,V,S>& x) +{ + T v; + v=(asin(x.t)); + return Rall1d<T,V,S>(v,x.grad/sqrt(1.0-sqr(x.t))); +} + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> acos(const Rall1d<T,V,S>& x) +{ + T v; + v=(acos(x.t)); + return Rall1d<T,V,S>(v,-x.grad/sqrt(1.0-sqr(x.t))); +} + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> abs(const Rall1d<T,V,S>& x) +{ + T v; + v=(Sign(x)); + return Rall1d<T,V,S>(v*x,v*x.grad); +} + + +template <class T,class V,class S> +INLINE S Norm(const Rall1d<T,V,S>& value) +{ + return Norm(value.t); +} + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> tanh(const Rall1d<T,V,S>& arg) +{ + T v(tanh(arg.t)); + return Rall1d<T,V,S>(v,arg.grad/sqr(cosh(arg.t))); +} + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> atan2(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x) +{ + T v(x.t*x.t+y.t*y.t); + return Rall1d<T,V,S>(atan2(y.t,x.t),(x.t*y.grad-y.t*x.grad)/v); +} + + +template <class T,class V,class S> +INLINE Rall1d<T,V,S> LinComb(S alfa,const Rall1d<T,V,S>& a, + const T& beta,const Rall1d<T,V,S>& b ) { + return Rall1d<T,V,S>( + LinComb(alfa,a.t,beta,b.t), + LinComb(alfa,a.grad,beta,b.grad) + ); +} + +template <class T,class V,class S> +INLINE void LinCombR(S alfa,const Rall1d<T,V,S>& a, + const T& beta,const Rall1d<T,V,S>& b,Rall1d<T,V,S>& result ) { + LinCombR(alfa, a.t, beta, b.t, result.t); + LinCombR(alfa, a.grad, beta, b.grad, result.grad); +} + + +template <class T,class V,class S> +INLINE void SetToZero(Rall1d<T,V,S>& value) + { + SetToZero(value.grad); + SetToZero(value.t); + } +template <class T,class V,class S> +INLINE void SetToIdentity(Rall1d<T,V,S>& value) + { + SetToIdentity(value.t); + SetToZero(value.grad); + } + +template <class T,class V,class S> +INLINE bool Equal(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x,double eps=epsilon) +{ + return (Equal(x.t,y.t,eps)&&Equal(x.grad,y.grad,eps)); +} + +} + + + +#endif diff --git a/intern/itasc/kdl/utilities/rall2d.h b/intern/itasc/kdl/utilities/rall2d.h new file mode 100644 index 00000000000..cbd9e70b04f --- /dev/null +++ b/intern/itasc/kdl/utilities/rall2d.h @@ -0,0 +1,538 @@ + +/***************************************************************************** + * \file + * class for automatic differentiation on scalar values and 1st + * derivatives and 2nd derivative. + * + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par Note + * VC6++ contains a bug, concerning the use of inlined friend functions + * in combination with namespaces. So, try to avoid inlined friend + * functions ! + * + * \par History + * - $log$ + * + * \par Release + * $Id$ + * $Name: $ + ****************************************************************************/ + +#ifndef Rall2D_H +#define Rall2D_H + +#include <math.h> +#include <assert.h> +#include "utility.h" + + +namespace KDL { + +/** + * Rall2d contains a value, and its gradient and its 2nd derivative, and defines an algebraic + * structure on this pair. + * This template class has 3 template parameters : + * - T contains the type of the value. + * - V contains the type of the gradient (can be a vector-like type). + * - S defines a scalar type that can operate on Rall1d. This is the type that + * is used to give back values of Norm() etc. + * + * S is usefull when you recurse a Rall1d object into itself to create a 2nd, 3th, 4th,.. + * derivatives. (e.g. Rall1d< Rall1d<double>, Rall1d<double>, double> ). + * + * S is always passed by value. + * + * \par Class Type + * Concrete implementation + */ +template <class T,class V=T,class S=T> +class Rall2d + { + public : + T t; //!< value + V d; //!< 1st derivative + V dd; //!< 2nd derivative + public : + // = Constructors + INLINE Rall2d() {} + + explicit INLINE Rall2d(typename TI<T>::Arg c) + {t=c;SetToZero(d);SetToZero(dd);} + + INLINE Rall2d(typename TI<T>::Arg tn,const V& afg):t(tn),d(afg) {SetToZero(dd);} + + INLINE Rall2d(typename TI<T>::Arg tn,const V& afg,const V& afg2):t(tn),d(afg),dd(afg2) {} + + // = Copy Constructor + INLINE Rall2d(const Rall2d<T,V,S>& r):t(r.t),d(r.d),dd(r.dd) {} + //if one defines this constructor, it's better optimized then the + //automatically generated one ( that one set's up a loop to copy + // word by word. + + // = Member functions to access internal structures : + INLINE T& Value() { + return t; + } + + INLINE V& D() { + return d; + } + + INLINE V& DD() { + return dd; + } + INLINE static Rall2d<T,V,S> Zero() { + Rall2d<T,V,S> tmp; + SetToZero(tmp); + return tmp; + } + INLINE static Rall2d<T,V,S> Identity() { + Rall2d<T,V,S> tmp; + SetToIdentity(tmp); + return tmp; + } + + // = assignment operators + INLINE Rall2d<T,V,S>& operator =(S c) + {t=c;SetToZero(d);SetToZero(dd);return *this;} + + INLINE Rall2d<T,V,S>& operator =(const Rall2d<T,V,S>& r) + {t=r.t;d=r.d;dd=r.dd;return *this;} + + INLINE Rall2d<T,V,S>& operator /=(const Rall2d<T,V,S>& rhs) + { + t /= rhs.t; + d = (d-t*rhs.d)/rhs.t; + dd= (dd - S(2)*d*rhs.d-t*rhs.dd)/rhs.t; + return *this; + } + + INLINE Rall2d<T,V,S>& operator *=(const Rall2d<T,V,S>& rhs) + { + t *= rhs.t; + d = (d*rhs.t+t*rhs.d); + dd = (dd*rhs.t+S(2)*d*rhs.d+t*rhs.dd); + return *this; + } + + INLINE Rall2d<T,V,S>& operator +=(const Rall2d<T,V,S>& rhs) + { + t +=rhs.t; + d +=rhs.d; + dd+=rhs.dd; + return *this; + } + + INLINE Rall2d<T,V,S>& operator -=(const Rall2d<T,V,S>& rhs) + { + t -= rhs.t; + d -= rhs.d; + dd -= rhs.dd; + return *this; + } + + INLINE Rall2d<T,V,S>& operator /=(S rhs) + { + t /= rhs; + d /= rhs; + dd /= rhs; + return *this; + } + + INLINE Rall2d<T,V,S>& operator *=(S rhs) + { + t *= rhs; + d *= rhs; + dd *= rhs; + return *this; + } + + INLINE Rall2d<T,V,S>& operator -=(S rhs) + { + t -= rhs; + return *this; + } + + INLINE Rall2d<T,V,S>& operator +=(S rhs) + { + t += rhs; + return *this; + } + + // = Operators between Rall2d objects +/* + friend INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs); + friend INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs); + friend INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs); + friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs); + friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& arg); + friend INLINE Rall2d<T,V,S> operator *(S s,const Rall2d<T,V,S>& v); + friend INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& v,S s); + friend INLINE Rall2d<T,V,S> operator +(S s,const Rall2d<T,V,S>& v); + friend INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& v,S s); + friend INLINE Rall2d<T,V,S> operator -(S s,const Rall2d<T,V,S>& v); + friend INLINE INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& v,S s); + friend INLINE Rall2d<T,V,S> operator /(S s,const Rall2d<T,V,S>& v); + friend INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& v,S s); + + // = Mathematical functions that operate on Rall2d objects + + friend INLINE Rall2d<T,V,S> exp(const Rall2d<T,V,S>& arg); + friend INLINE Rall2d<T,V,S> log(const Rall2d<T,V,S>& arg); + friend INLINE Rall2d<T,V,S> sin(const Rall2d<T,V,S>& arg); + friend INLINE Rall2d<T,V,S> cos(const Rall2d<T,V,S>& arg); + friend INLINE Rall2d<T,V,S> tan(const Rall2d<T,V,S>& arg); + friend INLINE Rall2d<T,V,S> sinh(const Rall2d<T,V,S>& arg); + friend INLINE Rall2d<T,V,S> cosh(const Rall2d<T,V,S>& arg); + friend INLINE Rall2d<T,V,S> tanh(const Rall2d<T,V,S>& arg); + friend INLINE Rall2d<T,V,S> sqr(const Rall2d<T,V,S>& arg); + friend INLINE Rall2d<T,V,S> pow(const Rall2d<T,V,S>& arg,double m) ; + friend INLINE Rall2d<T,V,S> sqrt(const Rall2d<T,V,S>& arg); + friend INLINE Rall2d<T,V,S> asin(const Rall2d<T,V,S>& arg); + friend INLINE Rall2d<T,V,S> acos(const Rall2d<T,V,S>& arg); + friend INLINE Rall2d<T,V,S> atan(const Rall2d<T,V,S>& x); + friend INLINE Rall2d<T,V,S> atan2(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x); + friend INLINE Rall2d<T,V,S> abs(const Rall2d<T,V,S>& x); + friend INLINE Rall2d<T,V,S> hypot(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x); + // returns sqrt(y*y+x*x), but is optimized for accuracy and speed. + friend INLINE S Norm(const Rall2d<T,V,S>& value) ; + // returns Norm( value.Value() ). + + // = Some utility functions to improve performance + // (should also be declared on primitive types to improve uniformity + friend INLINE Rall2d<T,V,S> LinComb(S alfa,const Rall2d<T,V,S>& a, + TI<T>::Arg beta,const Rall2d<T,V,S>& b ); + friend INLINE void LinCombR(S alfa,const Rall2d<T,V,S>& a, + TI<T>::Arg beta,const Rall2d<T,V,S>& b,Rall2d<T,V,S>& result ); + // = Setting value of a Rall2d object to 0 or 1 + friend INLINE void SetToZero(Rall2d<T,V,S>& value); + friend INLINE void SetToOne(Rall2d<T,V,S>& value); + // = Equality in an eps-interval + friend INLINE bool Equal(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x,double eps); + */ + }; + + + + + +// = Operators between Rall2d objects +template <class T,class V,class S> +INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs) + { + Rall2d<T,V,S> tmp; + tmp.t = lhs.t/rhs.t; + tmp.d = (lhs.d-tmp.t*rhs.d)/rhs.t; + tmp.dd= (lhs.dd-S(2)*tmp.d*rhs.d-tmp.t*rhs.dd)/rhs.t; + return tmp; + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs) + { + Rall2d<T,V,S> tmp; + tmp.t = lhs.t*rhs.t; + tmp.d = (lhs.d*rhs.t+lhs.t*rhs.d); + tmp.dd = (lhs.dd*rhs.t+S(2)*lhs.d*rhs.d+lhs.t*rhs.dd); + return tmp; + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs) + { + return Rall2d<T,V,S>(lhs.t+rhs.t,lhs.d+rhs.d,lhs.dd+rhs.dd); + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs) + { + return Rall2d<T,V,S>(lhs.t-rhs.t,lhs.d-rhs.d,lhs.dd-rhs.dd); + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& arg) + { + return Rall2d<T,V,S>(-arg.t,-arg.d,-arg.dd); + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> operator *(S s,const Rall2d<T,V,S>& v) + { + return Rall2d<T,V,S>(s*v.t,s*v.d,s*v.dd); + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& v,S s) + { + return Rall2d<T,V,S>(v.t*s,v.d*s,v.dd*s); + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> operator +(S s,const Rall2d<T,V,S>& v) + { + return Rall2d<T,V,S>(s+v.t,v.d,v.dd); + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& v,S s) + { + return Rall2d<T,V,S>(v.t+s,v.d,v.dd); + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> operator -(S s,const Rall2d<T,V,S>& v) + { + return Rall2d<T,V,S>(s-v.t,-v.d,-v.dd); + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& v,S s) + { + return Rall2d<T,V,S>(v.t-s,v.d,v.dd); + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> operator /(S s,const Rall2d<T,V,S>& rhs) + { + Rall2d<T,V,S> tmp; + tmp.t = s/rhs.t; + tmp.d = (-tmp.t*rhs.d)/rhs.t; + tmp.dd= (-S(2)*tmp.d*rhs.d-tmp.t*rhs.dd)/rhs.t; + return tmp; +} + + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& v,S s) + { + return Rall2d<T,V,S>(v.t/s,v.d/s,v.dd/s); + } + + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> exp(const Rall2d<T,V,S>& arg) + { + Rall2d<T,V,S> tmp; + tmp.t = exp(arg.t); + tmp.d = tmp.t*arg.d; + tmp.dd = tmp.d*arg.d+tmp.t*arg.dd; + return tmp; + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> log(const Rall2d<T,V,S>& arg) + { + Rall2d<T,V,S> tmp; + tmp.t = log(arg.t); + tmp.d = arg.d/arg.t; + tmp.dd = (arg.dd-tmp.d*arg.d)/arg.t; + return tmp; + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> sin(const Rall2d<T,V,S>& arg) + { + T v1 = sin(arg.t); + T v2 = cos(arg.t); + return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd - (v1*arg.d)*arg.d ); + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> cos(const Rall2d<T,V,S>& arg) + { + T v1 = cos(arg.t); + T v2 = -sin(arg.t); + return Rall2d<T,V,S>(v1,v2*arg.d, v2*arg.dd - (v1*arg.d)*arg.d); + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> tan(const Rall2d<T,V,S>& arg) + { + T v1 = tan(arg.t); + T v2 = S(1)+sqr(v1); + return Rall2d<T,V,S>(v1,v2*arg.d, v2*(arg.dd+(S(2)*v1*sqr(arg.d)))); + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> sinh(const Rall2d<T,V,S>& arg) + { + T v1 = sinh(arg.t); + T v2 = cosh(arg.t); + return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd + (v1*arg.d)*arg.d ); + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> cosh(const Rall2d<T,V,S>& arg) + { + T v1 = cosh(arg.t); + T v2 = sinh(arg.t); + return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd + (v1*arg.d)*arg.d ); + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> tanh(const Rall2d<T,V,S>& arg) + { + T v1 = tanh(arg.t); + T v2 = S(1)-sqr(v1); + return Rall2d<T,V,S>(v1,v2*arg.d, v2*(arg.dd-(S(2)*v1*sqr(arg.d)))); + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> sqr(const Rall2d<T,V,S>& arg) + { + return Rall2d<T,V,S>(arg.t*arg.t, + (S(2)*arg.t)*arg.d, + S(2)*(sqr(arg.d)+arg.t*arg.dd) + ); + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> pow(const Rall2d<T,V,S>& arg,double m) + { + Rall2d<T,V,S> tmp; + tmp.t = pow(arg.t,m); + T v2 = (m/arg.t)*tmp.t; + tmp.d = v2*arg.d; + tmp.dd = (S((m-1))/arg.t)*tmp.d*arg.d + v2*arg.dd; + return tmp; + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> sqrt(const Rall2d<T,V,S>& arg) + { + /* By inversion of sqr(x) :*/ + Rall2d<T,V,S> tmp; + tmp.t = sqrt(arg.t); + tmp.d = (S(0.5)/tmp.t)*arg.d; + tmp.dd = (S(0.5)*arg.dd-sqr(tmp.d))/tmp.t; + return tmp; + } + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> asin(const Rall2d<T,V,S>& arg) +{ + /* By inversion of sin(x) */ + Rall2d<T,V,S> tmp; + tmp.t = asin(arg.t); + T v = cos(tmp.t); + tmp.d = arg.d/v; + tmp.dd = (arg.dd+arg.t*sqr(tmp.d))/v; + return tmp; +} + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> acos(const Rall2d<T,V,S>& arg) +{ + /* By inversion of cos(x) */ + Rall2d<T,V,S> tmp; + tmp.t = acos(arg.t); + T v = -sin(tmp.t); + tmp.d = arg.d/v; + tmp.dd = (arg.dd+arg.t*sqr(tmp.d))/v; + return tmp; + +} + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> atan(const Rall2d<T,V,S>& x) +{ + /* By inversion of tan(x) */ + Rall2d<T,V,S> tmp; + tmp.t = atan(x.t); + T v = S(1)+sqr(x.t); + tmp.d = x.d/v; + tmp.dd = x.dd/v-(S(2)*x.t)*sqr(tmp.d); + return tmp; +} + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> atan2(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x) +{ + Rall2d<T,V,S> tmp; + tmp.t = atan2(y.t,x.t); + T v = sqr(y.t)+sqr(x.t); + tmp.d = (x.t*y.d-x.d*y.t)/v; + tmp.dd = ( x.t*y.dd-x.dd*y.t-S(2)*(x.t*x.d+y.t*y.d)*tmp.d ) / v; + return tmp; +} + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> abs(const Rall2d<T,V,S>& x) +{ + T v(Sign(x)); + return Rall2d<T,V,S>(v*x,v*x.d,v*x.dd); +} + +template <class T,class V,class S> +INLINE Rall2d<T,V,S> hypot(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x) +{ + Rall2d<T,V,S> tmp; + tmp.t = hypot(y.t,x.t); + tmp.d = (x.t*x.d+y.t*y.d)/tmp.t; + tmp.dd = (sqr(x.d)+x.t*x.dd+sqr(y.d)+y.t*y.dd-sqr(tmp.d))/tmp.t; + return tmp; +} +// returns sqrt(y*y+x*x), but is optimized for accuracy and speed. + +template <class T,class V,class S> +INLINE S Norm(const Rall2d<T,V,S>& value) +{ + return Norm(value.t); +} +// returns Norm( value.Value() ). + + +// (should also be declared on primitive types to improve uniformity +template <class T,class V,class S> +INLINE Rall2d<T,V,S> LinComb(S alfa,const Rall2d<T,V,S>& a, + const T& beta,const Rall2d<T,V,S>& b ) { + return Rall2d<T,V,S>( + LinComb(alfa,a.t,beta,b.t), + LinComb(alfa,a.d,beta,b.d), + LinComb(alfa,a.dd,beta,b.dd) + ); +} + +template <class T,class V,class S> +INLINE void LinCombR(S alfa,const Rall2d<T,V,S>& a, + const T& beta,const Rall2d<T,V,S>& b,Rall2d<T,V,S>& result ) { + LinCombR(alfa, a.t, beta, b.t, result.t); + LinCombR(alfa, a.d, beta, b.d, result.d); + LinCombR(alfa, a.dd, beta, b.dd, result.dd); +} + +template <class T,class V,class S> +INLINE void SetToZero(Rall2d<T,V,S>& value) + { + SetToZero(value.t); + SetToZero(value.d); + SetToZero(value.dd); + } + +template <class T,class V,class S> +INLINE void SetToIdentity(Rall2d<T,V,S>& value) + { + SetToZero(value.d); + SetToIdentity(value.t); + SetToZero(value.dd); + } + +template <class T,class V,class S> +INLINE bool Equal(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x,double eps=epsilon) +{ + return (Equal(x.t,y.t,eps)&& + Equal(x.d,y.d,eps)&& + Equal(x.dd,y.dd,eps) + ); +} + + +} + + +#endif diff --git a/intern/itasc/kdl/utilities/svd_eigen_HH.hpp b/intern/itasc/kdl/utilities/svd_eigen_HH.hpp new file mode 100644 index 00000000000..2bbb8df521f --- /dev/null +++ b/intern/itasc/kdl/utilities/svd_eigen_HH.hpp @@ -0,0 +1,309 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +//Based on the svd of the KDL-0.2 library by Erwin Aertbelien +#ifndef SVD_EIGEN_HH_HPP +#define SVD_EIGEN_HH_HPP + + +#include <Eigen/Array> +#include <algorithm> + +namespace KDL +{ + template<typename Scalar> inline Scalar PYTHAG(Scalar a,Scalar b) { + double at,bt,ct; + at = fabs(a); + bt = fabs(b); + if (at > bt ) { + ct=bt/at; + return Scalar(at*sqrt(1.0+ct*ct)); + } else { + if (bt==0) + return Scalar(0.0); + else { + ct=at/bt; + return Scalar(bt*sqrt(1.0+ct*ct)); + } + } + } + + + template<typename Scalar> inline Scalar SIGN(Scalar a,Scalar b) { + return ((b) >= Scalar(0.0) ? fabs(a) : -fabs(a)); + } + + /** + * svd calculation of boost ublas matrices + * + * @param A matrix<double>(mxn) + * @param U matrix<double>(mxn) + * @param S vector<double> n + * @param V matrix<double>(nxn) + * @param tmp vector<double> n + * @param maxiter defaults to 150 + * + * @return -2 if maxiter exceeded, 0 otherwise + */ + template<typename MatrixA, typename MatrixUV, typename VectorS> + int svd_eigen_HH( + const Eigen::MatrixBase<MatrixA>& A, + Eigen::MatrixBase<MatrixUV>& U, + Eigen::MatrixBase<VectorS>& S, + Eigen::MatrixBase<MatrixUV>& V, + Eigen::MatrixBase<VectorS>& tmp, + int maxiter=150) + { + //get the rows/columns of the matrix + const int rows = A.rows(); + const int cols = A.cols(); + + U = A; + + int i(-1),its(-1),j(-1),jj(-1),k(-1),nm=0; + int ppi(0); + bool flag; + e_scalar maxarg1,maxarg2,anorm(0),c(0),f(0),h(0),s(0),scale(0),x(0),y(0),z(0),g(0); + + g=scale=anorm=e_scalar(0.0); + + /* Householder reduction to bidiagonal form. */ + for (i=0;i<cols;i++) { + ppi=i+1; + tmp(i)=scale*g; + g=s=scale=e_scalar(0.0); + if (i<rows) { + // compute the sum of the i-th column, starting from the i-th row + for (k=i;k<rows;k++) scale += fabs(U(k,i)); + if (scale!=0) { + // multiply the i-th column by 1.0/scale, start from the i-th element + // sum of squares of column i, start from the i-th element + for (k=i;k<rows;k++) { + U(k,i) /= scale; + s += U(k,i)*U(k,i); + } + f=U(i,i); // f is the diag elem + g = -SIGN(e_scalar(sqrt(s)),f); + h=f*g-s; + U(i,i)=f-g; + for (j=ppi;j<cols;j++) { + // dot product of columns i and j, starting from the i-th row + for (s=0.0,k=i;k<rows;k++) s += U(k,i)*U(k,j); + f=s/h; + // copy the scaled i-th column into the j-th column + for (k=i;k<rows;k++) U(k,j) += f*U(k,i); + } + for (k=i;k<rows;k++) U(k,i) *= scale; + } + } + // save singular value + S(i)=scale*g; + g=s=scale=e_scalar(0.0); + if ((i <rows) && (i+1 != cols)) { + // sum of row i, start from columns i+1 + for (k=ppi;k<cols;k++) scale += fabs(U(i,k)); + if (scale!=0) { + for (k=ppi;k<cols;k++) { + U(i,k) /= scale; + s += U(i,k)*U(i,k); + } + f=U(i,ppi); + g = -SIGN(e_scalar(sqrt(s)),f); + h=f*g-s; + U(i,ppi)=f-g; + for (k=ppi;k<cols;k++) tmp(k)=U(i,k)/h; + for (j=ppi;j<rows;j++) { + for (s=0.0,k=ppi;k<cols;k++) s += U(j,k)*U(i,k); + for (k=ppi;k<cols;k++) U(j,k) += s*tmp(k); + } + for (k=ppi;k<cols;k++) U(i,k) *= scale; + } + } + maxarg1=anorm; + maxarg2=(fabs(S(i))+fabs(tmp(i))); + anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2; + } + /* Accumulation of right-hand transformations. */ + for (i=cols-1;i>=0;i--) { + if (i<cols-1) { + if (g) { + for (j=ppi;j<cols;j++) V(j,i)=(U(i,j)/U(i,ppi))/g; + for (j=ppi;j<cols;j++) { + for (s=0.0,k=ppi;k<cols;k++) s += U(i,k)*V(k,j); + for (k=ppi;k<cols;k++) V(k,j) += s*V(k,i); + } + } + for (j=ppi;j<cols;j++) V(i,j)=V(j,i)=0.0; + } + V(i,i)=1.0; + g=tmp(i); + ppi=i; + } + /* Accumulation of left-hand transformations. */ + for (i=cols-1<rows-1 ? cols-1:rows-1;i>=0;i--) { + ppi=i+1; + g=S(i); + for (j=ppi;j<cols;j++) U(i,j)=0.0; + if (g) { + g=e_scalar(1.0)/g; + for (j=ppi;j<cols;j++) { + for (s=0.0,k=ppi;k<rows;k++) s += U(k,i)*U(k,j); + f=(s/U(i,i))*g; + for (k=i;k<rows;k++) U(k,j) += f*U(k,i); + } + for (j=i;j<rows;j++) U(j,i) *= g; + } else { + for (j=i;j<rows;j++) U(j,i)=0.0; + } + ++U(i,i); + } + + /* Diagonalization of the bidiagonal form. */ + for (k=cols-1;k>=0;k--) { /* Loop over singular values. */ + for (its=1;its<=maxiter;its++) { /* Loop over allowed iterations. */ + flag=true; + for (ppi=k;ppi>=0;ppi--) { /* Test for splitting. */ + nm=ppi-1; /* Note that tmp(1) is always zero. */ + if ((fabs(tmp(ppi))+anorm) == anorm) { + flag=false; + break; + } + if ((fabs(S(nm)+anorm) == anorm)) break; + } + if (flag) { + c=e_scalar(0.0); /* Cancellation of tmp(l), if l>1: */ + s=e_scalar(1.); + for (i=ppi;i<=k;i++) { + f=s*tmp(i); + tmp(i)=c*tmp(i); + if ((fabs(f)+anorm) == anorm) break; + g=S(i); + h=PYTHAG(f,g); + S(i)=h; + h=e_scalar(1.0)/h; + c=g*h; + s=(-f*h); + for (j=0;j<rows;j++) { + y=U(j,nm); + z=U(j,i); + U(j,nm)=y*c+z*s; + U(j,i)=z*c-y*s; + } + } + } + z=S(k); + + if (ppi == k) { /* Convergence. */ + if (z < e_scalar(0.0)) { /* Singular value is made nonnegative. */ + S(k) = -z; + for (j=0;j<cols;j++) V(j,k)=-V(j,k); + } + break; + } + + x=S(ppi); /* Shift from bottom 2-by-2 minor: */ + nm=k-1; + y=S(nm); + g=tmp(nm); + h=tmp(k); + f=((y-z)*(y+z)+(g-h)*(g+h))/(e_scalar(2.0)*h*y); + + g=PYTHAG(f,e_scalar(1.0)); + f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x; + + /* Next QR transformation: */ + c=s=1.0; + for (j=ppi;j<=nm;j++) { + i=j+1; + g=tmp(i); + y=S(i); + h=s*g; + g=c*g; + z=PYTHAG(f,h); + tmp(j)=z; + c=f/z; + s=h/z; + f=x*c+g*s; + g=g*c-x*s; + h=y*s; + y=y*c; + for (jj=0;jj<cols;jj++) { + x=V(jj,j); + z=V(jj,i); + V(jj,j)=x*c+z*s; + V(jj,i)=z*c-x*s; + } + z=PYTHAG(f,h); + S(j)=z; + if (z) { + z=e_scalar(1.0)/z; + c=f*z; + s=h*z; + } + f=(c*g)+(s*y); + x=(c*y)-(s*g); + for (jj=0;jj<rows;jj++) { + y=U(jj,j); + z=U(jj,i); + U(jj,j)=y*c+z*s; + U(jj,i)=z*c-y*s; + } + } + tmp(ppi)=0.0; + tmp(k)=f; + S(k)=x; + } + } + + //Sort eigen values: + for (i=0; i<cols; i++){ + + double S_max = S(i); + int i_max = i; + for (j=i+1; j<cols; j++){ + double Sj = S(j); + if (Sj > S_max){ + S_max = Sj; + i_max = j; + } + } + if (i_max != i){ + /* swap eigenvalues */ + e_scalar tmp = S(i); + S(i)=S(i_max); + S(i_max)=tmp; + + /* swap eigenvectors */ + U.col(i).swap(U.col(i_max)); + V.col(i).swap(V.col(i_max)); + } + } + + + if (its == maxiter) + return (-2); + else + return (0); + } + +} +#endif diff --git a/intern/itasc/kdl/utilities/traits.h b/intern/itasc/kdl/utilities/traits.h new file mode 100644 index 00000000000..2656d633653 --- /dev/null +++ b/intern/itasc/kdl/utilities/traits.h @@ -0,0 +1,111 @@ +#ifndef KDLPV_TRAITS_H +#define KDLPV_TRAITS_H + +#include "utility.h" + + +// forwards declarations : +namespace KDL { + class Frame; + class Rotation; + class Vector; + class Twist; + class Wrench; + class FrameVel; + class RotationVel; + class VectorVel; + class TwistVel; +} + + +/** + * @brief Traits are traits classes to determine the type of a derivative of another type. + * + * For geometric objects the "geometric" derivative is chosen. For example the derivative of a Rotation + * matrix is NOT a 3x3 matrix containing the derivative of the elements of a rotation matrix. The derivative + * of the rotation matrix is a Vector corresponding the rotational velocity. Mostly used in template classes + * and routines to derive a correct type when needed. + * + * You can see this as a compile-time lookuptable to find the type of the derivative. + * + * Example + * \verbatim + Rotation R; + Traits<Rotation> dR; + \endverbatim + */ +template <typename T> +struct Traits { + typedef T valueType; + typedef T derivType; +}; + +template <> +struct Traits<KDL::Frame> { + typedef KDL::Frame valueType; + typedef KDL::Twist derivType; +}; +template <> +struct Traits<KDL::Twist> { + typedef KDL::Twist valueType; + typedef KDL::Twist derivType; +}; +template <> +struct Traits<KDL::Wrench> { + typedef KDL::Wrench valueType; + typedef KDL::Wrench derivType; +}; + +template <> +struct Traits<KDL::Rotation> { + typedef KDL::Rotation valueType; + typedef KDL::Vector derivType; +}; + +template <> +struct Traits<KDL::Vector> { + typedef KDL::Vector valueType; + typedef KDL::Vector derivType; +}; + +template <> +struct Traits<double> { + typedef double valueType; + typedef double derivType; +}; + +template <> +struct Traits<float> { + typedef float valueType; + typedef float derivType; +}; + +template <> +struct Traits<KDL::FrameVel> { + typedef KDL::Frame valueType; + typedef KDL::TwistVel derivType; +}; +template <> +struct Traits<KDL::TwistVel> { + typedef KDL::Twist valueType; + typedef KDL::TwistVel derivType; +}; + +template <> +struct Traits<KDL::RotationVel> { + typedef KDL::Rotation valueType; + typedef KDL::VectorVel derivType; +}; + +template <> +struct Traits<KDL::VectorVel> { + typedef KDL::Vector valueType; + typedef KDL::VectorVel derivType; +}; + + + +#endif + + + diff --git a/intern/itasc/kdl/utilities/utility.cpp b/intern/itasc/kdl/utilities/utility.cpp new file mode 100644 index 00000000000..1ab9cb6f83d --- /dev/null +++ b/intern/itasc/kdl/utilities/utility.cpp @@ -0,0 +1,21 @@ +/** @file utility.cpp + * @author Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * @version + * ORO_Geometry V0.2 + * + * @par history + * - changed layout of the comments to accomodate doxygen + */ + +#include "utility.h" + +namespace KDL { + +int STREAMBUFFERSIZE=10000; +int MAXLENFILENAME = 255; +const double PI= 3.1415926535897932384626433832795; +const double deg2rad = 0.01745329251994329576923690768488; +const double rad2deg = 57.2957795130823208767981548141052; +double epsilon = 0.000001; +double epsilon2 = 0.000001*0.000001; +} diff --git a/intern/itasc/kdl/utilities/utility.h b/intern/itasc/kdl/utilities/utility.h new file mode 100644 index 00000000000..7151792536e --- /dev/null +++ b/intern/itasc/kdl/utilities/utility.h @@ -0,0 +1,299 @@ +/***************************************************************************** + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id$ + * $Name: $ + * \file + * Included by most lrl-files to provide some general + * functions and macro definitions. + * + * \par history + * - changed layout of the comments to accomodate doxygen + */ + + +#ifndef KDL_UTILITY_H +#define KDL_UTILITY_H + +#include "kdl-config.h" +#include <cstdlib> +#include <cassert> +#include <cmath> + + +///////////////////////////////////////////////////////////// +// configurable options for the frames library. + +#ifdef KDL_INLINE + #ifdef _MSC_VER + // Microsoft Visual C + #define IMETHOD __forceinline + #else + // Some other compiler, e.g. gcc + #define IMETHOD inline + #endif +#else + #define IMETHOD +#endif + + + +//! turn on or off frames bounds checking. If turned on, assert() can still +//! be turned off with -DNDEBUG. +#ifdef KDL_INDEX_CHECK + #define FRAMES_CHECKI(a) assert(a) +#else + #define FRAMES_CHECKI(a) +#endif + + +namespace KDL { + +#ifdef __GNUC__ + // so that sin,cos can be overloaded and complete + // resolution of overloaded functions work. + using ::sin; + using ::cos; + using ::exp; + using ::log; + using ::sin; + using ::cos; + using ::tan; + using ::sinh; + using ::cosh; + using ::pow; + using ::sqrt; + using ::atan; + using ::hypot; + using ::asin; + using ::acos; + using ::tanh; + using ::atan2; +#endif +#ifndef __GNUC__ + //only real solution : get Rall1d and varia out of namespaces. + #pragma warning (disable:4786) + + inline double sin(double a) { + return ::sin(a); + } + + inline double cos(double a) { + return ::cos(a); + } + inline double exp(double a) { + return ::exp(a); + } + inline double log(double a) { + return ::log(a); + } + inline double tan(double a) { + return ::tan(a); + } + inline double cosh(double a) { + return ::cosh(a); + } + inline double sinh(double a) { + return ::sinh(a); + } + inline double sqrt(double a) { + return ::sqrt(a); + } + inline double atan(double a) { + return ::atan(a); + } + inline double acos(double a) { + return ::acos(a); + } + inline double asin(double a) { + return ::asin(a); + } + inline double tanh(double a) { + return ::tanh(a); + } + inline double pow(double a,double b) { + return ::pow(a,b); + } + inline double atan2(double a,double b) { + return ::atan2(a,b); + } +#endif + + + + + +/** + * Auxiliary class for argument types (Trait-template class ) + * + * Is used to pass doubles by value, and arbitrary objects by const reference. + * This is TWICE as fast (2 x less memory access) and avoids bugs in VC6++ concerning + * the assignment of the result of intrinsic functions to const double&-typed variables, + * and optimization on. + */ +template <class T> +class TI +{ + public: + typedef const T& Arg; //!< Arg is used for passing the element to a function. +}; + +template <> +class TI<double> { +public: + typedef double Arg; +}; + +template <> +class TI<int> { +public: + typedef int Arg; +}; + + + + + +/** + * /note linkage + * Something fishy about the difference between C++ and C + * in C++ const values default to INTERNAL linkage, in C they default + * to EXTERNAL linkage. Here the constants should have EXTERNAL linkage + * because they, for at least some of them, can be changed by the user. + * If you want to explicitly declare internal linkage, use "static". + */ +//! +extern int STREAMBUFFERSIZE; + +//! maximal length of a file name +extern int MAXLENFILENAME; + +//! the value of pi +extern const double PI; + +//! the value pi/180 +extern const double deg2rad; + +//! the value 180/pi +extern const double rad2deg; + +//! default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001. +extern double epsilon; + +//! power or 2 of epsilon +extern double epsilon2; + +//! the number of derivatives used in the RN-... objects. +extern int VSIZE; + + + +#ifndef _MFC_VER +#undef max +inline double max(double a,double b) { + if (b<a) + return a; + else + return b; +} + +#undef min +inline double min(double a,double b) { + if (b<a) + return b; + else + return a; +} +#endif + + +#ifdef _MSC_VER + //#pragma inline_depth( 255 ) + //#pragma inline_recursion( on ) + #define INLINE __forceinline + //#define INLINE inline +#else + #define INLINE inline +#endif + + +inline double LinComb(double alfa,double a, + double beta,double b ) { + return alfa*a+beta*b; +} + +inline void LinCombR(double alfa,double a, + double beta,double b,double& result ) { + result=alfa*a+beta*b; + } + +//! to uniformly set double, RNDouble,Vector,... objects to zero in template-classes +inline void SetToZero(double& arg) { + arg=0; +} + +//! to uniformly set double, RNDouble,Vector,... objects to the identity element in template-classes +inline void SetToIdentity(double& arg) { + arg=1; +} + +inline double sign(double arg) { + return (arg<0)?(-1):(1); +} + +inline double sqr(double arg) { return arg*arg;} +inline double Norm(double arg) { + return fabs( (double)arg ); +} + + +#if defined(__WIN32__) && !defined(__GNUC__) +inline double hypot(double y,double x) { return ::_hypot(y,x);} +inline double abs(double x) { return ::fabs(x);} +#endif + +// compares whether 2 doubles are equal in an eps-interval. +// Does not check whether a or b represents numbers +// On VC6, if a/b is -INF, it returns false; +inline bool Equal(double a,double b,double eps=epsilon) +{ + double tmp=(a-b); + return ((eps>tmp)&& (tmp>-eps) ); +} + +inline void random(double& a) { + a = 1.98*rand()/(double)RAND_MAX -0.99; +} + +inline void posrandom(double& a) { + a = 0.001+0.99*rand()/(double)RAND_MAX; +} + +inline double diff(double a,double b,double dt) { + return (b-a)/dt; +} +//inline float diff(float a,float b,double dt) { +//return (b-a)/dt; +//} +inline double addDelta(double a,double da,double dt) { + return a+da*dt; +} + +//inline float addDelta(float a,float da,double dt) { +// return a+da*dt; +//} + + +} + + + +#endif diff --git a/intern/itasc/kdl/utilities/utility_io.cpp b/intern/itasc/kdl/utilities/utility_io.cpp new file mode 100644 index 00000000000..994567dfdfc --- /dev/null +++ b/intern/itasc/kdl/utilities/utility_io.cpp @@ -0,0 +1,208 @@ +/***************************************************************************** + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id$ + * $Name: $ + * \todo + * make IO routines more robust against the differences between DOS/UNIX end-of-line style. + ****************************************************************************/ + + +#include "utility_io.h" +#include "error.h" + +#include <stdlib.h> +#include <ctype.h> +#include <string.h> + +namespace KDL { + +// +// _functions are private functions +// + + void _check_istream(std::istream& is) + { + if ((!is.good())&&(is.eof()) ) + { + throw Error_BasicIO_File(); + } + } +// Eats until the end of the line + int _EatUntilEndOfLine( std::istream& is, int* countp=NULL) { + int ch; + int count; + count = 0; + do { + ch = is.get(); + count++; + _check_istream(is); + } while (ch!='\n'); + if (countp!=NULL) *countp = count; + return ch; +} + +// Eats until the end of the comment + int _EatUntilEndOfComment( std::istream& is, int* countp=NULL) { + int ch; + int count; + count = 0; + int prevch; + ch = 0; + do { + prevch = ch; + ch = is.get(); + count++; + _check_istream(is); + if ((prevch=='*')&&(ch=='/')) { + break; + } + } while (true); + if (countp!=NULL) *countp = count; + ch = is.get(); + return ch; +} + +// Eats space-like characters and comments +// possibly returns the number of space-like characters eaten. +int _EatSpace( std::istream& is,int* countp=NULL) { + int ch; + int count; + count=-1; + do { + _check_istream(is); + + ch = is.get(); + count++; + if (ch == '#') { + ch = _EatUntilEndOfLine(is,&count); + } + if (ch == '/') { + ch = is.get(); + if (ch == '/') { + ch = _EatUntilEndOfLine(is,&count); + } else if (ch == '*') { + ch = _EatUntilEndOfComment(is,&count); + } else { + is.putback(ch); + ch = '/'; + } + } + } while ((ch==' ')||(ch=='\n')||(ch=='\t')); + if (countp!=NULL) *countp = count; + return ch; +} + + + +// Eats whites, returns, tabs and the delim character +// Checks wether delim char. is encountered. +void Eat( std::istream& is, int delim ) +{ + int ch; + ch=_EatSpace(is); + if (ch != delim) { + throw Error_BasicIO_Exp_Delim(); + } + ch=_EatSpace(is); + is.putback(ch); +} + +// Eats whites, returns, tabs and the delim character +// Checks wether delim char. is encountered. +// EatEnd does not eat all space-like char's at the end. +void EatEnd( std::istream& is, int delim ) +{ + int ch; + ch=_EatSpace(is); + if (ch != delim) { + throw Error_BasicIO_Exp_Delim(); + } +} + + + +// For each space in descript, this routine eats whites,tabs, and newlines (at least one) +// There should be no consecutive spaces in the description. +// for each letter in descript, its reads the corresponding letter in the output +// the routine is case insensitive. + + +// Simple routine, enough for our purposes. +// works with ASCII chars +inline char Upper(char ch) +{ + /*if (('a'<=ch)&&(ch<='z')) + return (ch-'a'+'A'); + else + return ch; + */ + return toupper(ch); +} + +void Eat(std::istream& is,const char* descript) +{ + // eats whites before word + char ch; + char chdescr; + ch=_EatSpace(is); + is.putback(ch); + const char* p; + p = descript; + while ((*p)!=0) { + chdescr = (char)Upper(*p); + if (chdescr==' ') { + int count=0; + ch=_EatSpace(is,&count); + is.putback(ch); + if (count==0) { + throw Error_BasicIO_Not_A_Space(); + } + } else { + ch=(char)is.get(); + if (chdescr!=Upper(ch)) { + throw Error_BasicIO_Unexpected(); + } + } + p++; + } + +} + + + +void EatWord(std::istream& is,const char* delim,char* storage,int maxsize) +{ + int ch; + char* p; + int size; + // eat white before word + ch=_EatSpace(is); + p = storage; + size=0; + int count = 0; + while ((count==0)&&(strchr(delim,ch)==NULL)) { + *p = (char) toupper(ch); + ++p; + if (size==maxsize) { + throw Error_BasicIO_ToBig(); + } + _check_istream(is); + ++size; + //ch = is.get(); + ch =_EatSpace(is,&count); + } + *p=0; + is.putback(ch); +} + + +} diff --git a/intern/itasc/kdl/utilities/utility_io.h b/intern/itasc/kdl/utilities/utility_io.h new file mode 100644 index 00000000000..2a71ce870a3 --- /dev/null +++ b/intern/itasc/kdl/utilities/utility_io.h @@ -0,0 +1,79 @@ +/***************************************************************************** + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id$ + * $Name: $ + * + * \file utility_io.h + * Included by most lrl-files to provide some general + * functions and macro definitions related to file/stream I/O. + */ + +#ifndef KDL_UTILITY_IO_H_84822 +#define KDL_UTILITY_IO_H_84822 + +//#include <kdl/kdl-config.h> + + +// Standard includes +#include <iostream> +#include <iomanip> +#include <fstream> + + +namespace KDL { + + +/** + * checks validity of basic io of is + */ +void _check_istream(std::istream& is); + + +/** + * Eats characters of the stream until the character delim is encountered + * @param is a stream + * @param delim eat until this character is encountered + */ +void Eat(std::istream& is, int delim ); + +/** + * Eats characters of the stream as long as they satisfy the description in descript + * @param is a stream + * @param descript description string. A sequence of spaces, tabs, + * new-lines and comments is regarded as 1 space in the description string. + */ +void Eat(std::istream& is,const char* descript); + +/** + * Eats a word of the stream delimited by the letters in delim or space(tabs...) + * @param is a stream + * @param delim a string containing the delimmiting characters + * @param storage for returning the word + * @param maxsize a word can be maximally maxsize-1 long. + */ +void EatWord(std::istream& is,const char* delim,char* storage,int maxsize); + +/** + * Eats characters of the stream until the character delim is encountered + * similar to Eat(is,delim) but spaces at the end are not read. + * @param is a stream + * @param delim eat until this character is encountered + */ +void EatEnd( std::istream& is, int delim ); + + + + +} + + +#endif diff --git a/intern/itasc/make/msvc_9_0/itasc.vcproj b/intern/itasc/make/msvc_9_0/itasc.vcproj new file mode 100644 index 00000000000..f4a81079da0 --- /dev/null +++ b/intern/itasc/make/msvc_9_0/itasc.vcproj @@ -0,0 +1,539 @@ +<?xml version="1.0" encoding="Windows-1252"?>
+<VisualStudioProject
+ ProjectType="Visual C++"
+ Version="9,00"
+ Name="INT_itasc"
+ ProjectGUID="{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}"
+ RootNamespace="itasc"
+ Keyword="Win32Proj"
+ TargetFrameworkVersion="196613"
+ >
+ <Platforms>
+ <Platform
+ Name="Win32"
+ />
+ </Platforms>
+ <ToolFiles>
+ </ToolFiles>
+ <Configurations>
+ <Configuration
+ Name="Blender Debug|Win32"
+ OutputDirectory="..\..\..\..\..\build\msvc_9\intern\itasc\debug"
+ IntermediateDirectory="..\..\..\..\..\build\msvc_9\intern\itasc\debug"
+ ConfigurationType="4"
+ InheritedPropertySheets="$(VCInstallDir)VCProjectDefaults\UpgradeFromVC71.vsprops"
+ UseOfMFC="0"
+ ATLMinimizesCRunTimeLibraryUsage="false"
+ CharacterSet="2"
+ >
+ <Tool
+ Name="VCPreBuildEventTool"
+ />
+ <Tool
+ Name="VCCustomBuildTool"
+ />
+ <Tool
+ Name="VCXMLDataGeneratorTool"
+ />
+ <Tool
+ Name="VCWebServiceProxyGeneratorTool"
+ />
+ <Tool
+ Name="VCMIDLTool"
+ />
+ <Tool
+ Name="VCCLCompilerTool"
+ Optimization="0"
+ AdditionalIncludeDirectories="..\..\..\..\extern\Eigen2"
+ PreprocessorDefinitions="WIN32;_DEBUG;_LIB"
+ MinimalRebuild="false"
+ BasicRuntimeChecks="3"
+ RuntimeLibrary="1"
+ EnableEnhancedInstructionSet="0"
+ FloatingPointModel="0"
+ UsePrecompiledHeader="0"
+ PrecompiledHeaderFile="..\..\..\..\..\build\msvc_9\intern\itasc\debug\itasc.pch"
+ AssemblerListingLocation="..\..\..\..\..\build\msvc_9\intern\itasc\debug\"
+ ObjectFile="..\..\..\..\..\build\msvc_9\intern\itasc\debug\"
+ ProgramDataBaseFileName="..\..\..\..\..\build\msvc_9\intern\itasc\debug\"
+ WarningLevel="3"
+ DebugInformationFormat="3"
+ CompileAs="0"
+ />
+ <Tool
+ Name="VCManagedResourceCompilerTool"
+ />
+ <Tool
+ Name="VCResourceCompilerTool"
+ />
+ <Tool
+ Name="VCPreLinkEventTool"
+ />
+ <Tool
+ Name="VCLibrarianTool"
+ OutputFile="..\..\..\..\..\build\msvc_9\libs\intern\debug\libitasc.lib"
+ SuppressStartupBanner="true"
+ />
+ <Tool
+ Name="VCALinkTool"
+ />
+ <Tool
+ Name="VCXDCMakeTool"
+ />
+ <Tool
+ Name="VCBscMakeTool"
+ />
+ <Tool
+ Name="VCFxCopTool"
+ />
+ <Tool
+ Name="VCPostBuildEventTool"
+ />
+ </Configuration>
+ <Configuration
+ Name="Blender Release|Win32"
+ OutputDirectory="..\..\..\..\..\build\msvc_9\intern\itasc"
+ IntermediateDirectory="..\..\..\..\..\build\msvc_9\intern\itasc"
+ ConfigurationType="4"
+ InheritedPropertySheets="$(VCInstallDir)VCProjectDefaults\UpgradeFromVC71.vsprops"
+ UseOfMFC="0"
+ ATLMinimizesCRunTimeLibraryUsage="false"
+ CharacterSet="2"
+ WholeProgramOptimization="1"
+ >
+ <Tool
+ Name="VCPreBuildEventTool"
+ />
+ <Tool
+ Name="VCCustomBuildTool"
+ />
+ <Tool
+ Name="VCXMLDataGeneratorTool"
+ />
+ <Tool
+ Name="VCWebServiceProxyGeneratorTool"
+ />
+ <Tool
+ Name="VCMIDLTool"
+ />
+ <Tool
+ Name="VCCLCompilerTool"
+ Optimization="2"
+ EnableIntrinsicFunctions="true"
+ AdditionalIncludeDirectories="..\..\..\..\extern\Eigen2"
+ PreprocessorDefinitions="WIN32;NDEBUG;_LIB"
+ RuntimeLibrary="0"
+ EnableFunctionLevelLinking="true"
+ EnableEnhancedInstructionSet="0"
+ FloatingPointModel="2"
+ UsePrecompiledHeader="0"
+ AssemblerListingLocation="$(IntDir)\"
+ WarningLevel="3"
+ CompileAs="0"
+ />
+ <Tool
+ Name="VCManagedResourceCompilerTool"
+ />
+ <Tool
+ Name="VCResourceCompilerTool"
+ />
+ <Tool
+ Name="VCPreLinkEventTool"
+ />
+ <Tool
+ Name="VCLibrarianTool"
+ OutputFile="..\..\..\..\..\build\msvc_9\libs\intern\libitasc.lib"
+ SuppressStartupBanner="true"
+ />
+ <Tool
+ Name="VCALinkTool"
+ />
+ <Tool
+ Name="VCXDCMakeTool"
+ />
+ <Tool
+ Name="VCBscMakeTool"
+ />
+ <Tool
+ Name="VCFxCopTool"
+ />
+ <Tool
+ Name="VCPostBuildEventTool"
+ />
+ </Configuration>
+ </Configurations>
+ <References>
+ </References>
+ <Files>
+ <Filter
+ Name="kdl"
+ >
+ <Filter
+ Name="Header Files"
+ >
+ <File
+ RelativePath="..\..\kdl\chain.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\chainfksolver.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\chainfksolverpos_recursive.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\chainjnttojacsolver.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\frameacc.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\frameacc.inl"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\frames.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\frames.inl"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\frames_io.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\framevel.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\framevel.inl"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\inertia.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\jacobian.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\jntarray.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\jntarrayacc.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\jntarrayvel.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\joint.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\kinfam_io.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\segment.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\tree.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\treefksolver.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\treefksolverpos_recursive.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\treejnttojacsolver.hpp"
+ >
+ </File>
+ <Filter
+ Name="Utilities"
+ >
+ <File
+ RelativePath="..\..\kdl\utilities\error.h"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\utilities\error_stack.h"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\utilities\kdl-config.h"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\utilities\rall1d.h"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\utilities\rall2d.h"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\utilities\svd_eigen_HH.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\utilities\traits.h"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\utilities\utility.h"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\utilities\utility_io.h"
+ >
+ </File>
+ </Filter>
+ </Filter>
+ <Filter
+ Name="Source Files"
+ >
+ <File
+ RelativePath="..\..\kdl\chain.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\chainfksolverpos_recursive.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\chainjnttojacsolver.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\frameacc.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\frames.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\frames_io.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\framevel.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\inertia.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\jacobian.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\jntarray.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\jntarrayacc.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\jntarrayvel.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\joint.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\kinfam_io.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\segment.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\tree.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\treefksolverpos_recursive.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\treejnttojacsolver.cpp"
+ >
+ </File>
+ <Filter
+ Name="Utilities"
+ >
+ <File
+ RelativePath="..\..\kdl\utilities\error_stack.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\utilities\utility.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\kdl\utilities\utility_io.cpp"
+ >
+ </File>
+ </Filter>
+ </Filter>
+ </Filter>
+ <Filter
+ Name="itasc"
+ >
+ <Filter
+ Name="Header Files"
+ Filter="h;hpp;hxx;hm;inl;inc;xsd"
+ UniqueIdentifier="{93995380-89BD-4b04-88EB-625FBE52EBFB}"
+ >
+ <File
+ RelativePath="..\..\Armature.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\Cache.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\ConstraintSet.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\ControlledObject.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\CopyPose.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\Distance.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\eigen_types.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\FixedObject.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\MovingFrame.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\Object.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\Scene.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\Solver.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\UncontrolledObject.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\WDLSSolver.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\WorldObject.hpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\WSDLSSolver.hpp"
+ >
+ </File>
+ </Filter>
+ <Filter
+ Name="Source Files"
+ Filter="cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx"
+ UniqueIdentifier="{4FC737F1-C7A5-4376-A066-2A32D752A2FF}"
+ >
+ <File
+ RelativePath="..\..\Armature.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\Cache.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\ConstraintSet.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\ControlledObject.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\CopyPose.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\Distance.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\eigen_types.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\FixedObject.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\MovingFrame.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\Scene.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\UncontrolledObject.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\WDLSSolver.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\WorldObject.cpp"
+ >
+ </File>
+ <File
+ RelativePath="..\..\WSDLSSolver.cpp"
+ >
+ </File>
+ </Filter>
+ </Filter>
+ </Files>
+ <Globals>
+ </Globals>
+</VisualStudioProject>
diff --git a/intern/itasc/ublas_types.hpp b/intern/itasc/ublas_types.hpp new file mode 100644 index 00000000000..bf9bdcc26f2 --- /dev/null +++ b/intern/itasc/ublas_types.hpp @@ -0,0 +1,82 @@ +/* + * ublas_types.hpp + * + * Created on: Jan 5, 2009 + * Author: rubensmits + */ + +#ifndef UBLAS_TYPES_HPP_ +#define UBLAS_TYPES_HPP_ + +#include <boost/numeric/ublas/matrix.hpp> +#include <boost/numeric/ublas/vector.hpp> +#include <boost/numeric/ublas/matrix_proxy.hpp> +#include <boost/numeric/ublas/vector_proxy.hpp> +#include "kdl/frames.hpp" +#include "kdl/tree.hpp" +#include "kdl/chain.hpp" +#include "kdl/jacobian.hpp" +#include "kdl/jntarray.hpp" + + +namespace iTaSC{ + +namespace ublas = boost::numeric::ublas; +using KDL::Twist; +using KDL::Frame; +using KDL::Joint; +using KDL::Inertia; +using KDL::SegmentMap; +using KDL::Tree; +using KDL::JntArray; +using KDL::Jacobian; +using KDL::Segment; +using KDL::Rotation; +using KDL::Vector; +using KDL::Chain; + +#define u_scalar double +#define u_vector ublas::vector<u_scalar> +#define u_zero_vector ublas::zero_vector<u_scalar> +#define u_matrix ublas::matrix<u_scalar> +#define u_matrix6 ublas::matrix<u_scalar,6,6> +#define u_identity_matrix ublas::identity_matrix<u_scalar> +#define u_scalar_vector ublas::scalar_vector<u_scalar> +#define u_zero_matrix ublas::zero_matrix<u_scalar> +#define u_vector6 ublas::bounded_vector<u_scalar,6> + +inline static int changeBase(const u_matrix& J_in, const Frame& T, u_matrix& J_out) { + + if (J_out.size1() != 6 || J_in.size1() != 6 || J_in.size2() != J_out.size2()) + return -1; + for (unsigned int j = 0; j < J_in.size2(); ++j) { + ublas::matrix_column<const u_matrix > Jj_in = column(J_in,j); + ublas::matrix_column<u_matrix > Jj_out = column(J_out,j); + Twist arg; + for(unsigned int i=0;i<6;++i) + arg(i)=Jj_in(i); + Twist tmp(T*arg); + for(unsigned int i=0;i<6;++i) + Jj_out(i)=tmp(i); + } + return 0; +} +inline static int changeBase(const ublas::matrix_range<u_matrix >& J_in, const Frame& T, ublas::matrix_range<u_matrix >& J_out) { + + if (J_out.size1() != 6 || J_in.size1() != 6 || J_in.size2() != J_out.size2()) + return -1; + for (unsigned int j = 0; j < J_in.size2(); ++j) { + ublas::matrix_column<const ublas::matrix_range<u_matrix > > Jj_in = column(J_in,j); + ublas::matrix_column<ublas::matrix_range<u_matrix > > Jj_out = column(J_out,j); + Twist arg; + for(unsigned int i=0;i<6;++i) + arg(i)=Jj_in(i); + Twist tmp(T*arg); + for(unsigned int i=0;i<6;++i) + Jj_out(i)=tmp(i); + } + return 0; +} + +} +#endif /* UBLAS_TYPES_HPP_ */ |