From 06b8ddca8f1c72073a3eb6d45368b71e26e012e3 Mon Sep 17 00:00:00 2001 From: Bastien Montagne Date: Fri, 20 Sep 2024 11:33:23 +0200 Subject: [PATCH] Cleanup: make format. --- intern/itasc/Armature.cpp | 1275 ++++++++-------- intern/itasc/Armature.hpp | 246 +-- intern/itasc/Cache.cpp | 1114 +++++++------- intern/itasc/Cache.hpp | 281 ++-- intern/itasc/ConstraintSet.cpp | 297 ++-- intern/itasc/ConstraintSet.hpp | 187 ++- intern/itasc/ControlledObject.cpp | 70 +- intern/itasc/ControlledObject.hpp | 103 +- intern/itasc/CopyPose.cpp | 823 +++++----- intern/itasc/CopyPose.hpp | 177 ++- intern/itasc/Distance.cpp | 522 +++---- intern/itasc/Distance.hpp | 74 +- intern/itasc/FixedObject.cpp | 85 +- intern/itasc/FixedObject.hpp | 43 +- intern/itasc/MovingFrame.cpp | 216 +-- intern/itasc/MovingFrame.hpp | 57 +- intern/itasc/Object.hpp | 78 +- intern/itasc/Scene.cpp | 20 +- intern/itasc/Scene.hpp | 158 +- intern/itasc/Solver.hpp | 33 +- intern/itasc/UncontrolledObject.cpp | 41 +- intern/itasc/UncontrolledObject.hpp | 42 +- intern/itasc/WDLSSolver.cpp | 160 +- intern/itasc/WDLSSolver.hpp | 64 +- intern/itasc/WSDLSSolver.cpp | 230 +-- intern/itasc/WSDLSSolver.hpp | 54 +- intern/itasc/WorldObject.cpp | 15 +- intern/itasc/WorldObject.hpp | 24 +- intern/itasc/eigen_types.cpp | 4 +- intern/itasc/eigen_types.hpp | 86 +- intern/itasc/kdl/chain.cpp | 84 +- intern/itasc/kdl/chain.hpp | 127 +- intern/itasc/kdl/chainfksolver.hpp | 133 +- .../itasc/kdl/chainfksolverpos_recursive.cpp | 57 +- .../itasc/kdl/chainfksolverpos_recursive.hpp | 33 +- intern/itasc/kdl/chainjnttojacsolver.cpp | 99 +- intern/itasc/kdl/chainjnttojacsolver.hpp | 65 +- intern/itasc/kdl/frameacc.cpp | 6 +- intern/itasc/kdl/frameacc.hpp | 368 +++-- intern/itasc/kdl/frames.cpp | 584 +++---- intern/itasc/kdl/frames.hpp | 1345 ++++++++--------- intern/itasc/kdl/frames_io.cpp | 505 ++++--- intern/itasc/kdl/frames_io.hpp | 48 +- intern/itasc/kdl/framevel.cpp | 7 +- intern/itasc/kdl/framevel.hpp | 533 +++---- intern/itasc/kdl/inertia.cpp | 26 +- intern/itasc/kdl/inertia.hpp | 54 +- intern/itasc/kdl/jacobian.cpp | 205 ++- intern/itasc/kdl/jacobian.hpp | 61 +- intern/itasc/kdl/jntarray.cpp | 252 ++- intern/itasc/kdl/jntarray.hpp | 363 +++-- intern/itasc/kdl/jntarrayacc.cpp | 279 ++-- intern/itasc/kdl/jntarrayacc.hpp | 69 +- intern/itasc/kdl/jntarrayvel.cpp | 155 +- intern/itasc/kdl/jntarrayvel.hpp | 55 +- intern/itasc/kdl/joint.cpp | 266 ++-- intern/itasc/kdl/joint.hpp | 204 +-- intern/itasc/kdl/kinfam_io.cpp | 113 +- intern/itasc/kdl/kinfam_io.hpp | 63 +- intern/itasc/kdl/segment.cpp | 68 +- intern/itasc/kdl/segment.hpp | 217 +-- intern/itasc/kdl/tree.cpp | 164 +- intern/itasc/kdl/tree.hpp | 273 ++-- intern/itasc/kdl/treefksolver.hpp | 122 +- .../itasc/kdl/treefksolverpos_recursive.cpp | 77 +- .../itasc/kdl/treefksolverpos_recursive.hpp | 42 +- intern/itasc/kdl/treejnttojacsolver.cpp | 107 +- intern/itasc/kdl/treejnttojacsolver.hpp | 27 +- intern/itasc/kdl/utilities/error.h | 401 +++-- intern/itasc/kdl/utilities/error_stack.cpp | 54 +- intern/itasc/kdl/utilities/error_stack.h | 27 +- intern/itasc/kdl/utilities/kdl-config.h | 1 - intern/itasc/kdl/utilities/rall1d.h | 767 +++++----- intern/itasc/kdl/utilities/rall2d.h | 878 ++++++----- intern/itasc/kdl/utilities/svd_eigen_HH.hpp | 575 +++---- intern/itasc/kdl/utilities/traits.h | 128 +- intern/itasc/kdl/utilities/utility.cpp | 14 +- intern/itasc/kdl/utilities/utility.h | 360 ++--- intern/itasc/kdl/utilities/utility_io.cpp | 288 ++-- intern/itasc/kdl/utilities/utility_io.h | 40 +- intern/itasc/ublas_types.hpp | 95 +- 81 files changed, 8951 insertions(+), 8512 deletions(-) diff --git a/intern/itasc/Armature.cpp b/intern/itasc/Armature.cpp index a86d09977c2..f5128bf9d02 100644 --- a/intern/itasc/Armature.cpp +++ b/intern/itasc/Armature.cpp @@ -8,8 +8,8 @@ #include "Armature.hpp" #include -#include #include +#include namespace iTaSC { @@ -19,129 +19,141 @@ static const unsigned int constraintCacheSize = 5; #endif 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), +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), #if 0 m_yCTs(0), #endif - m_qKdl(), - m_oldqKdl(), - m_newqKdl(), - m_qdotKdl(), - m_jac(NULL), - m_armlength(0.0), - m_jacsolver(NULL), - m_fksolver(NULL) + 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(); + 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) +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; - } + 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); + 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)); + 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 @@ -152,35 +164,36 @@ void Armature::initCache(Cache *_cache) // store the initial cache position at timestamp 0 pushConstraints(0); #endif - pushQ(0); - } + 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; - } + 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(double)); - m_qCTs = timestamp; - // changing the joint => recompute the jacobian - updateJacobian(); - } - return (item) ? true : false; - } - return true; + 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(double)); + m_qCTs = timestamp; + // changing the joint => recompute the jacobian + updateJacobian(); + } + return (item) ? true : false; + } + return true; } #if 0 void Armature::pushConstraints(CacheTS timestamp) @@ -229,550 +242,606 @@ bool Armature::popConstraints(CacheTS timestamp) } #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) +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; + 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; dofsecond.segment.getJoint(); - if (q_size < p_joint->getNDof()) - return false; - p_tip = &sit->second.segment.getFrameToTip(); - for (unsigned int dof=0; dofgetNDof(); dof++) { - (&q_rest)[dof] = m_joints[sit->second.q_nr+dof].rest; - (&q)[dof] = m_qKdl[sit->second.q_nr+dof]; - } - return true; + 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 maxDelta) - maxDelta = delta; - delta = twist.vel.Norm(); - if (delta > maxDelta) - maxDelta = delta; - } - return maxDelta; + 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) +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++; + 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) +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; - 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; + 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; + 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) +int Armature::addEndEffector(const std::string &name) { - const SegmentMap& segments = m_tree.getSegments(); - if (segments.find(name) == segments.end()) - return -1; + 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++; + 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++; } bool Armature::finalize() { - unsigned int i, j, c; - if (m_finalized) - return true; - if (m_njoint == 0) - return false; - initialize(m_njoint, m_noutput, m_neffector); - for (i=c=0; iv_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; ifirst != "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; - return true; + unsigned int i, j, c; + if (m_finalized) + return true; + if (m_njoint == 0) + return false; + 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::value_type const *sit = m_tree.getSegmentPtr(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; + return true; } -void Armature::pushCache(const Timestamp& timestamp) +void Armature::pushCache(const Timestamp ×tamp) { - if (!timestamp.substep && timestamp.cache) { - pushQ(timestamp.cacheTimestamp); - //pushConstraints(timestamp.cacheTimestamp); - } + if (!timestamp.substep && timestamp.cache) { + pushQ(timestamp.cacheTimestamp); + // pushConstraints(timestamp.cacheTimestamp); + } } -bool Armature::setJointArray(const KDL::JntArray& joints) +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; + if (!m_finalized) + return false; + if (joints.rows() != m_qKdl.rows()) + return false; + m_qKdl = joints; + updateJacobian(); + return true; } -const KDL::JntArray& Armature::getJointArray() +const KDL::JntArray &Armature::getJointArray() { - return m_qKdl; + return m_qKdl; } -bool Armature::updateJoint(const Timestamp& timestamp, JointLockCallback& callback) +bool Armature::updateJoint(const Timestamp ×tamp, 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; + if (!m_finalized) + return false; - for (q_nr=0; q_nrlocked) { - 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; indof; 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; indof; 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; indof; 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; - } + for (q_nr = 0; q_nr < m_nq; ++q_nr) + qdot[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_nrlocked) - unlocked++; - q_nr += joint->ndof; - } - // if all joints have been locked no need to run the solver again - return (unlocked) ? locked : false; + // 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){ +void Armature::updateKinematics(const Timestamp ×tamp) +{ - //Integrate m_qdot - if (!m_finalized) - return; + // 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. + // 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; eeJntToCart(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;jJntToCart(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) +const Frame &Armature::getPose(const unsigned int ee) { - if (!m_finalized) - return F_identity; - return (ee >= m_nee) ? F_identity : m_effectors[ee].pose; + 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) +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; + 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) +void Armature::updateControlOutput(const Timestamp ×tamp) { - if (!m_finalized) - return; + if (!m_finalized) + return; + if (!timestamp.substep && !timestamp.reiterate && timestamp.interpolate) { + popQ(timestamp.cacheTimestamp); + // popConstraints(timestamp.cacheTimestamp); + } - 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; + } + } - 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; isegment->second.q_nr; iv_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; iv_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); - } - } + // 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) +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 ( ; constraintIdv_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; iv_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; iv_nr; i++) - m_Wy(pConstraint->y_nr+i) = pConstraint->values[i].alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/; - } - } - return true; -} - + 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; } +} // namespace iTaSC diff --git a/intern/itasc/Armature.hpp b/intern/itasc/Armature.hpp index 16b0535fc24..0d9e0e93063 100644 --- a/intern/itasc/Armature.hpp +++ b/intern/itasc/Armature.hpp @@ -9,132 +9,168 @@ #ifndef ARMATURE_HPP_ #define ARMATURE_HPP_ -#include "ControlledObject.hpp" #include "ConstraintSet.hpp" -#include "kdl/treejnttojacsolver.hpp" +#include "ControlledObject.hpp" #include "kdl/treefksolverpos_recursive.hpp" +#include "kdl/treejnttojacsolver.hpp" #include namespace iTaSC { -class Armature: public iTaSC::ControlledObject { -public: - Armature(); - virtual ~Armature(); +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); + 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 bool finalize(); + virtual bool 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 int addEndEffector(const std::string &name); + virtual const Frame &getPose(const unsigned int end_effector); + virtual bool updateJoint(const Timestamp ×tamp, JointLockCallback &callback); + virtual void updateKinematics(const Timestamp ×tamp); + virtual void pushCache(const Timestamp ×tamp); + virtual void updateControlOutput(const Timestamp ×tamp); + 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; - } + 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 EffectorList; + 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 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 JointConstraintList; + 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 JointConstraintList; - struct Joint_struct { - KDL::Joint::JointType type; - unsigned short ndof; - bool useLimit; - bool locked; - double rest; - double min; - double max; + 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 JointList; - -protected: - virtual void updateJacobian(); + 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 JointList; -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; + 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; #if 0 CacheTS m_yCTs; #endif - JntArray m_qKdl; - JntArray m_oldqKdl; - JntArray m_newqKdl; - JntArray m_qdotKdl; - Jacobian* m_jac; - double m_armlength; + 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); + 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); }; -} +} // namespace iTaSC #endif /* ARMATURE_HPP_ */ diff --git a/intern/itasc/Cache.cpp b/intern/itasc/Cache.cpp index 367c69befda..c471b65d3f3 100644 --- a/intern/itasc/Cache.cpp +++ b/intern/itasc/Cache.cpp @@ -6,619 +6,647 @@ * \ingroup intern_itasc */ -#include +#include "Cache.hpp" #include #include #include -#include "Cache.hpp" +#include namespace iTaSC { CacheEntry::~CacheEntry() { - for (unsigned int id=0; id < m_count; id++) - m_channelArray[id].clear(); - if (m_channelArray) - free(m_channelArray); + 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) +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); + // 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 *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 *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; + 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) +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; + 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) +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); + 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() {} Cache::~Cache() { - CacheMap::iterator it; - for (it=m_cache.begin(); it!=m_cache.end(); it=m_cache.begin()) { - deleteDevice(it->first); - } + 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; + CacheMap::iterator it = m_cache.find(device); + CacheEntry *entry; + CacheChannel *channel; + unsigned int id; - if (maxItemSize > 0x3FFF0) - return -1; + 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) { - delete entry; - return -1; - } - } else { - entry = it->second; - } - // locate a channel with the same name and reuse - for (channel=entry->m_channelArray, id=0; idm_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; idm_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; + 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) { + delete entry; + 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; + 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; + 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; + 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; + 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; + 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; chm_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; - } + 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) +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; + 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; + 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; + 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); + 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; + 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; + 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; + 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) +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; + 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; } -} +} // namespace iTaSC diff --git a/intern/itasc/Cache.hpp b/intern/itasc/Cache.hpp index ffe2a84114f..50a4a4c1590 100644 --- a/intern/itasc/Cache.hpp +++ b/intern/itasc/Cache.hpp @@ -13,51 +13,57 @@ 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 +#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 alignment gap after an item header */ -#define CACHE_ITEM_GAPB(item) (unsigned int)(((size_t)item+sizeof(CacheItem))&(sizeof(void*)-1)) +#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)) +#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) +#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)<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) +#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; +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)); } + Timestamp() + { + memset(this, 0, sizeof(Timestamp)); + } }; /* utility function to return second timestamp to millisecond */ -inline void setCacheTimestamp(Timestamp& timestamp) +inline void setCacheTimestamp(Timestamp ×tamp) { - if (timestamp.realTimestamp < 0.0 || timestamp.realTimestamp > 4294967.295) - timestamp.cacheTimestamp = 0; - else - timestamp.cacheTimestamp = (CacheTS)(timestamp.realTimestamp*1000.0+0.5); + 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. @@ -68,13 +74,13 @@ The device must specify the largest cache item (limited to 256Kb) so that the ca 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 +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. +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 +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. @@ -85,108 +91,113 @@ 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 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); +class Cache { + private: + /* map between device and cache entry. + Dynamically updated when more devices create cache channels */ + typedef std::map CacheMap; + CacheMap m_cache; + const CacheItem *getCurrentCacheItemInternal(const void *device, int channel, CacheTS timestamp); - /* 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); + 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); - /* 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); + /* 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); - /* 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); + /* 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); - /* 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); + /* 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 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); + /* 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 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 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 + 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 +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 @@ -198,31 +209,29 @@ struct CacheItem // 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 +// 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 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; + 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 +#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 }; - -} +} // namespace iTaSC #endif /* CACHE_HPP_ */ diff --git a/intern/itasc/ConstraintSet.cpp b/intern/itasc/ConstraintSet.cpp index de3701917ba..0a0b1eb9cfb 100644 --- a/intern/itasc/ConstraintSet.cpp +++ b/intern/itasc/ConstraintSet.cpp @@ -11,164 +11,179 @@ 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) +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); + 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) +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); + m_maxDeltaChi = e_scalar(0.52); } -void ConstraintSet::reset(unsigned int _nc,double accuracy,unsigned int maximum_iterations) +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; + 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() { +ConstraintSet::~ConstraintSet() {} -} - -void ConstraintSet::modelUpdate(Frame& _external_pose,const Timestamp& timestamp) +void ConstraintSet::modelUpdate(Frame &_external_pose, const Timestamp ×tamp) { - m_chi+=m_chidot*timestamp.realTimestep; - m_externalPose = _external_pose; + 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++; + // 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) +double ConstraintSet::getMaxTimestep(double ×tep) { - e_scalar maxChidot = m_chidot.array().abs().maxCoeff(); - if (timestep*maxChidot > m_maxDeltaChi) { - timestep = m_maxDeltaChi/maxChidot; - } - return timestep; + e_scalar maxChidot = m_chidot.array().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(); +bool ConstraintSet::initialise(Frame &init_pose) +{ + m_externalPose = init_pose; + // get current Jf + updateJacobian(); - unsigned int iter=0; - while(iter 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); } - if (iter 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) namespace iTaSC { enum ConstraintAction { - ACT_NONE= 0, - ACT_VALUE= 1, - ACT_VELOCITY= 2, - ACT_TOLERANCE= 4, - ACT_FEEDBACK= 8, - ACT_ALPHA= 16 + 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) {} + 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) {} + 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); +typedef bool (*ConstraintCallback)(const Timestamp ×tamp, + 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; + 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); + friend class Scene; + virtual void modelUpdate(KDL::Frame &_external_pose, const Timestamp ×tamp); + virtual void updateKinematics(const Timestamp ×tamp) = 0; + virtual void pushCache(const Timestamp ×tamp) = 0; + virtual void updateJacobian() = 0; + virtual void updateControlOutput(const Timestamp ×tamp) = 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 ×tep); + public: + ConstraintSet(unsigned int nc, double accuracy, unsigned int maximum_iterations); + ConstraintSet(); + virtual ~ConstraintSet(); -public: - ConstraintSet(unsigned int nc,double accuracy,unsigned int maximum_iterations); - ConstraintSet(); - virtual ~ConstraintSet(); + EIGEN_MAKE_ALIGNED_OPERATOR_NEW - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + virtual bool registerCallback(ConstraintCallback _function, void *_param) + { + m_constraintCallback = _function; + m_constraintParam = _param; + return true; + } - 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_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_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 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;} + virtual unsigned int getNrOfConstraints() + { + return m_nc; + }; + void substep(bool _substep) + { + m_substep = _substep; + } + bool substep() + { + return m_substep; + } }; -} +} // namespace iTaSC #endif /* CONSTRAINTSET_HPP_ */ diff --git a/intern/itasc/ControlledObject.cpp b/intern/itasc/ControlledObject.cpp index 5b79fa84ccb..8041c01a503 100644 --- a/intern/itasc/ControlledObject.cpp +++ b/intern/itasc/ControlledObject.cpp @@ -8,55 +8,51 @@ #include "ControlledObject.hpp" - namespace iTaSC { -ControlledObject::ControlledObject(): - Object(Controlled),m_nq(0),m_nc(0),m_nee(0) +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); + // 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) +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)); + 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 +const e_matrix &ControlledObject::getJq(unsigned int ee) const { - assert(m_nq > 0); - return m_JqArray[(ee>m_nee)?m_nee:ee]; + assert(m_nq > 0); + return m_JqArray[(ee > m_nee) ? m_nee : ee]; } -double ControlledObject::getMaxTimestep(double& timestep) +double ControlledObject::getMaxTimestep(double ×tep) { - e_scalar maxQdot = m_qdot.array().abs().maxCoeff(); - if (timestep*maxQdot > m_maxDeltaQ) { - timestep = m_maxDeltaQ/maxQdot; - } - return timestep; + e_scalar maxQdot = m_qdot.array().abs().maxCoeff(); + if (timestep * maxQdot > m_maxDeltaQ) { + timestep = m_maxDeltaQ / maxQdot; + } + return timestep; } -} +} // namespace iTaSC diff --git a/intern/itasc/ControlledObject.hpp b/intern/itasc/ControlledObject.hpp index 098a56af320..1f6d2a17533 100644 --- a/intern/itasc/ControlledObject.hpp +++ b/intern/itasc/ControlledObject.hpp @@ -9,63 +9,92 @@ #ifndef CONTROLLEDOBJECT_HPP_ #define CONTROLLEDOBJECT_HPP_ -#include "kdl/frames.hpp" #include "eigen_types.hpp" +#include "kdl/frames.hpp" -#include "Object.hpp" #include "ConstraintSet.hpp" +#include "Object.hpp" #include namespace iTaSC { -#define CONSTRAINT_ID_ALL ((unsigned int)-1) +#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 m_JqArray; -public: - ControlledObject(); - virtual ~ControlledObject(); + 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 m_JqArray; - class JointLockCallback { - public: - JointLockCallback() {} - virtual ~JointLockCallback() {} + public: + ControlledObject(); + virtual ~ControlledObject(); - // 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; - }; + class JointLockCallback { + public: + JointLockCallback() {} + virtual ~JointLockCallback() {} - virtual void initialize(unsigned int _nq,unsigned int _nc, unsigned int _nee); + // 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; + }; - // 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 void initialize(unsigned int _nq, unsigned int _nc, unsigned int _nee); - virtual const e_vector& getControlOutput() const{return m_ydot;} + // returns true when a joint has been locked via the callback and the solver must run again + virtual bool updateJoint(const Timestamp ×tamp, JointLockCallback &callback) = 0; + virtual void updateControlOutput(const Timestamp ×tamp) = 0; + virtual void setJointVelocity(const e_vector qdot_in) + { + m_qdot = qdot_in; + }; + virtual double getMaxTimestep(double ×tep); + virtual bool setControlParameter(unsigned int constraintId, + unsigned int valueId, + ConstraintAction action, + e_scalar value, + double timestep = 0.0) = 0; - virtual const e_matrix& getJq(unsigned int ee) const; + virtual const e_vector &getControlOutput() const + { + return m_ydot; + } - virtual const e_matrix& getCq() const{return m_Cq;}; + virtual const e_matrix &getJq(unsigned int ee) const; - virtual e_matrix& getWq() {return m_Wq;}; - virtual void setWq(const e_matrix& Wq_in){m_Wq = Wq_in;}; + virtual const e_matrix &getCq() const + { + return m_Cq; + }; - virtual const e_vector& getWy() const {return m_Wy;}; + virtual e_matrix &getWq() + { + return m_Wq; + }; + virtual void setWq(const e_matrix &Wq_in) + { + m_Wq = Wq_in; + }; - virtual const unsigned int getNrOfCoordinates(){return m_nq;}; - virtual const unsigned int getNrOfConstraints(){return m_nc;}; + 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; + }; }; -} +} // namespace iTaSC #endif /* CONTROLLEDOBJECT_HPP_ */ diff --git a/intern/itasc/CopyPose.cpp b/intern/itasc/CopyPose.cpp index d74d5397b05..a0c639ddd35 100644 --- a/intern/itasc/CopyPose.cpp +++ b/intern/itasc/CopyPose.cpp @@ -11,471 +11,500 @@ #include #include -namespace iTaSC -{ +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) +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); + 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() +CopyPose::~CopyPose() {} + +bool CopyPose::initialise(Frame &init_pose) { + m_externalPose = m_internalPose = init_pose; + updateJacobian(); + return true; } -bool CopyPose::initialise(Frame& init_pose) +void CopyPose::modelUpdate(Frame &_external_pose, const Timestamp ×tamp) { - 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(); + 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); - } + 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) +double *CopyPose::pushValues(double *item, ControlState *_state, unsigned int mask) { - ControlState::ControlValue* _yval; - int i; + ControlState::ControlValue *_yval; + int i; - *item++ = _state->alpha; - *item++ = _state->K; - *item++ = _state->tolerance; + *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; + 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; + 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) +double *CopyPose::restoreValues(double *item, + ConstraintValues *_values, + ControlState *_state, + unsigned int mask) { - ConstraintSingleValue* _data; - ControlState::ControlValue* _yval; - int i, j; + ConstraintSingleValue *_data; + ControlState::ControlValue *_yval; + int i, j; - _values->alpha = _state->alpha = *item++; - _values->feedback = _state->K = *item++; - _values->tolerance = _state->tolerance = *item++; + _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; ialpha/*/(_state->tolerance*_state->K)*/; - if (m_outputDynamic & mask) { - _data->yd = _yval->yd = *item++; - _data->yddot = _yval->yddot = *item++; - } - _data++; - _yval++; - i++; - } - } - return 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; - } + 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; + } + return found; } -void CopyPose::interpolateOutput(ControlState* _state, unsigned int mask, const Timestamp& timestamp) +void CopyPose::interpolateOutput(ControlState *_state, + unsigned int mask, + const Timestamp ×tamp) { - ControlState::ControlValue* _yval; - int i; + 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++; - } - } + 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) +void CopyPose::pushCache(const Timestamp ×tamp) { - if (!timestamp.substep && timestamp.cache) { - pushPose(timestamp.cacheTimestamp); - } + if (!timestamp.substep && timestamp.cache) { + pushPose(timestamp.cacheTimestamp); + } } -void CopyPose::updateKinematics(const Timestamp& timestamp) +void CopyPose::updateKinematics(const Timestamp ×tamp) { - 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); + 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(); + // 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) +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; + 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_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++; } - if ((_values->action & ACT_TOLERANCE) && _values->tolerance > KDL::epsilon) { - _state->tolerance = _values->tolerance; - action |= ACT_TOLERANCE; + } +} + +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->action & ACT_FEEDBACK) && _values->feedback > KDL::epsilon) { - _state->K = _values->feedback; - action |= ACT_FEEDBACK; + if (_values->id >= ID_ROTATION && _values->id <= ID_ROTATIONZ && + (m_outputControl & CTL_ROTATION)) + { + updateState(_values, &m_rot, CTL_ROTATIONX, timestep); } - for (i=_state->firsty, j=_state->firsty+_state->ny, _yval=_state->output; ialpha/*/(_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++; - } - } + _values++; + _nvalues--; + } + return true; } - -bool CopyPose::setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep) +void CopyPose::updateValues(Vector &vel, + ConstraintValues *_values, + ControlState *_state, + unsigned int mask) { - 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; + 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::updateValues(Vector& vel, ConstraintValues* _values, ControlState* _state, unsigned int mask) +void CopyPose::updateOutput(Vector &vel, 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++; - } - } + 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::updateOutput(Vector& vel, ControlState* _state, unsigned int mask) +void CopyPose::updateControlOutput(const Timestamp ×tamp) { - 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<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<yddot+_state->K*coef*(_yval->yd-vel(j)); - _yval++; - i++; - } - } + // 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); + } } -void CopyPose::updateControlOutput(const Timestamp& timestamp) +const ConstraintValues *CopyPose::getControlParameters(unsigned int *_nvalues) { - //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); - } + 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; } -const ConstraintValues* CopyPose::getControlParameters(unsigned int* _nvalues) +double CopyPose::getMaxTimestep(double ×tep) { - 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; + // 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).array().abs().maxCoeff(); + if (timestep * maxChidot > m_maxDeltaChi) { + timestep = m_maxDeltaChi / maxChidot; + } + return timestep; } -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).array().abs().maxCoeff(); - if (timestep*maxChidot > m_maxDeltaChi) { - timestep = m_maxDeltaChi/maxChidot; - } - return timestep; -} - -} +} // namespace iTaSC diff --git a/intern/itasc/CopyPose.hpp b/intern/itasc/CopyPose.hpp index cefe5fc6d34..565fca49b78 100644 --- a/intern/itasc/CopyPose.hpp +++ b/intern/itasc/CopyPose.hpp @@ -10,91 +10,114 @@ #define COPYPOSE_H_ #include "ConstraintSet.hpp" -namespace iTaSC{ +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); +class CopyPose : public iTaSC::ConstraintSet { + protected: + virtual void updateKinematics(const Timestamp ×tamp); + virtual void pushCache(const Timestamp ×tamp); + virtual void updateJacobian(); + virtual bool initialise(Frame &init_pose); + virtual void initCache(Cache *_cache); + virtual void updateControlOutput(const Timestamp ×tamp); + virtual void modelUpdate(Frame &_external_pose, const Timestamp ×tamp); + virtual double getMaxTimestep(double ×tep); -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, - }; + 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(); + // 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); + 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; + 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); + 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 ×tamp); }; -} +} // namespace iTaSC #endif /* COPYROTATION_H_ */ diff --git a/intern/itasc/Distance.cpp b/intern/itasc/Distance.cpp index 9d9f5e6065a..59861950020 100644 --- a/intern/itasc/Distance.cpp +++ b/intern/itasc/Distance.cpp @@ -11,312 +11,322 @@ #include #include -namespace iTaSC -{ +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; +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) +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_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; + 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; + delete m_fksolver; + delete m_jacsolver; } -bool Distance::computeChi(Frame& pose) +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; + 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) +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; + // 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; + 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); - } + 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; - } + 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; + 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) +void Distance::pushCache(const Timestamp ×tamp) { - if (!timestamp.substep && timestamp.cache) - pushDist(timestamp.cacheTimestamp); + if (!timestamp.substep && timestamp.cache) + pushDist(timestamp.cacheTimestamp); } -void Distance::updateKinematics(const Timestamp& timestamp) +void Distance::updateKinematics(const Timestamp ×tamp) { - 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); + 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]; + 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); + 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) +bool Distance::setControlParameters(struct ConstraintValues *_values, + unsigned int _nvalues, + double timestep) { - int action = 0; - int i; - ConstraintSingleValue* _data; + 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; + 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) +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; + *(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) +void Distance::updateControlOutput(const Timestamp ×tamp) { - 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; + 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; } -} +} // namespace iTaSC diff --git a/intern/itasc/Distance.hpp b/intern/itasc/Distance.hpp index 6c9ab814306..309cbbcf1ec 100644 --- a/intern/itasc/Distance.hpp +++ b/intern/itasc/Distance.hpp @@ -14,50 +14,50 @@ #include "kdl/chainfksolverpos_recursive.hpp" #include "kdl/chainjnttojacsolver.hpp" -namespace iTaSC -{ +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(); +class Distance : public iTaSC::ConstraintSet { + protected: + virtual void updateKinematics(const Timestamp ×tamp); + virtual void pushCache(const Timestamp ×tamp); + virtual void updateJacobian(); + virtual bool initialise(Frame &init_pose); + virtual void initCache(Cache *_cache); + virtual void updateControlOutput(const Timestamp ×tamp); + 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(); + 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); + 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; + 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); + void pushDist(CacheTS timestamp); + bool popDist(CacheTS timestamp); - double m_alpha,m_yddot,m_yd,m_nextyd,m_nextyddot,m_K,m_tolerance; + double m_alpha, m_yddot, m_yd, m_nextyd, m_nextyddot, m_K, m_tolerance; }; -} +} // namespace iTaSC #endif /* DISTANCE_HPP_ */ diff --git a/intern/itasc/FixedObject.cpp b/intern/itasc/FixedObject.cpp index c5bb511154b..ae3e96da4c9 100644 --- a/intern/itasc/FixedObject.cpp +++ b/intern/itasc/FixedObject.cpp @@ -8,65 +8,62 @@ #include "FixedObject.hpp" -namespace iTaSC{ +namespace iTaSC { +FixedObject::FixedObject() : UncontrolledObject(), m_finalized(false), m_nframe(0) {} -FixedObject::FixedObject():UncontrolledObject(), - m_finalized(false), m_nframe(0) +FixedObject::~FixedObject() { + m_frameArray.clear(); } -FixedObject::~FixedObject() +int FixedObject::addFrame(const std::string &name, const Frame &frame) { - m_frameArray.clear(); + 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::addFrame(const std::string& name, const Frame& frame) +int FixedObject::addEndEffector(const std::string &name) { - if (m_finalized) - return -1; - FrameList::iterator it; - unsigned int i; - for (i=0, it=m_frameArray.begin(); ifirst == 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(); ifirst == name) { - return i; - } - } - return -1; + // 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; } bool FixedObject::finalize() { - if (m_finalized) - return true; - initialize(0, m_nframe); - m_finalized = true; - return true; + if (m_finalized) + return true; + initialize(0, m_nframe); + m_finalized = true; + return true; } -const Frame& FixedObject::getPose(const unsigned int frameIndex) +const Frame &FixedObject::getPose(const unsigned int frameIndex) { - if (frameIndex < m_nframe) { - return m_frameArray[frameIndex].second; - } else { - return F_identity; - } + if (frameIndex < m_nframe) { + return m_frameArray[frameIndex].second; + } + else { + return F_identity; + } } -} +} // namespace iTaSC diff --git a/intern/itasc/FixedObject.hpp b/intern/itasc/FixedObject.hpp index 284d702c08c..8e17abe08ec 100644 --- a/intern/itasc/FixedObject.hpp +++ b/intern/itasc/FixedObject.hpp @@ -12,35 +12,34 @@ #include "UncontrolledObject.hpp" #include +namespace iTaSC { -namespace iTaSC{ +class FixedObject : public UncontrolledObject { + public: + FixedObject(); + virtual ~FixedObject(); -class FixedObject: public UncontrolledObject { -public: - FixedObject(); - virtual ~FixedObject(); + int addFrame(const std::string &name, const Frame &frame); - int addFrame(const std::string& name, const Frame& frame); + virtual void updateCoordinates(const Timestamp ×tamp){}; + virtual int addEndEffector(const std::string &name); + virtual bool finalize(); + virtual const Frame &getPose(const unsigned int frameIndex); + virtual void updateKinematics(const Timestamp ×tamp){}; + virtual void pushCache(const Timestamp ×tamp){}; + virtual void initCache(Cache *_cache){}; - virtual void updateCoordinates(const Timestamp& timestamp) {}; - virtual int addEndEffector(const std::string& name); - virtual bool 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() {} -protected: - virtual void updateJacobian() {} -private: - typedef std::vector > FrameList; - - bool m_finalized; - unsigned int m_nframe; - FrameList m_frameArray; + private: + typedef std::vector> FrameList; + bool m_finalized; + unsigned int m_nframe; + FrameList m_frameArray; }; -} +} // namespace iTaSC #endif /* FIXEDOBJECT_H_ */ diff --git a/intern/itasc/MovingFrame.cpp b/intern/itasc/MovingFrame.cpp index 9b36fac4219..04b4d595f7e 100644 --- a/intern/itasc/MovingFrame.cpp +++ b/intern/itasc/MovingFrame.cpp @@ -8,151 +8,161 @@ #include "MovingFrame.hpp" #include -namespace iTaSC{ +namespace iTaSC { -static const unsigned int frameCacheSize = (sizeof(((Frame*)0)->p.data)+sizeof(((Frame*)0)->M.data))/sizeof(double); +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) +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); + m_internalPose = m_nextPose = frame; + initialize(6, 1); + e_matrix &Ju = m_JuArray[0]; + Ju = e_identity_matrix(6, 6); } -MovingFrame::~MovingFrame() -{ -} +MovingFrame::~MovingFrame() {} bool MovingFrame::finalize() { - updateJacobian(); - return true; + updateJacobian(); + return true; } 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); - } + 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)); + 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; - } + m_cache->addCacheVectorIfDifferent( + this, m_poseCCh, timestamp, buf, frameCacheSize, KDL::epsilon); + m_poseCTs = timestamp; + } } // load pose just preceding 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; + 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) +bool MovingFrame::setFrame(const Frame &frame) { - m_internalPose = m_nextPose = frame; - return true; + m_internalPose = m_nextPose = frame; + return true; } -bool MovingFrame::setCallback(MovingFrameCallback _function, void* _param) +bool MovingFrame::setCallback(MovingFrameCallback _function, void *_param) { - m_function = _function; - m_param = _param; - return true; + m_function = _function; + m_param = _param; + return true; } -void MovingFrame::updateCoordinates(const Timestamp& timestamp) +void MovingFrame::updateCoordinates(const Timestamp ×tamp) { - // 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(); - } - } + // 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) +void MovingFrame::pushCache(const Timestamp ×tamp) { - if (!timestamp.substep && timestamp.cache) - pushInternalFrame(timestamp.cacheTimestamp); + if (!timestamp.substep && timestamp.cache) + pushInternalFrame(timestamp.cacheTimestamp); } -void MovingFrame::updateKinematics(const Timestamp& timestamp) +void MovingFrame::updateKinematics(const Timestamp ×tamp) { - 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); + 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]; + 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; + // 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; } -} +} // namespace iTaSC diff --git a/intern/itasc/MovingFrame.hpp b/intern/itasc/MovingFrame.hpp index 0a277686e84..ac9e7337759 100644 --- a/intern/itasc/MovingFrame.hpp +++ b/intern/itasc/MovingFrame.hpp @@ -12,42 +12,41 @@ #include "UncontrolledObject.hpp" #include +namespace iTaSC { -namespace iTaSC{ +typedef bool (*MovingFrameCallback)(const Timestamp ×tamp, + const Frame &_current, + Frame &_next, + void *param); -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(); -class MovingFrame: public UncontrolledObject { -public: - MovingFrame(const Frame& frame=F_identity); - virtual ~MovingFrame(); + bool setFrame(const Frame &frame); + bool setCallback(MovingFrameCallback _function, void *_param); - bool setFrame(const Frame& frame); - bool setCallback(MovingFrameCallback _function, void* _param); + virtual void updateCoordinates(const Timestamp ×tamp); + virtual void updateKinematics(const Timestamp ×tamp); + virtual void pushCache(const Timestamp ×tamp); + virtual void initCache(Cache *_cache); + virtual bool finalize(); - virtual void updateCoordinates(const Timestamp& timestamp); - virtual void updateKinematics(const Timestamp& timestamp); - virtual void pushCache(const Timestamp& timestamp); - virtual void initCache(Cache *_cache); - virtual bool finalize(); -protected: - virtual void updateJacobian(); + 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; + 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; }; -} +} // namespace iTaSC #endif /* MOVINGFRAME_H_ */ diff --git a/intern/itasc/Object.hpp b/intern/itasc/Object.hpp index e54a6c95f11..4f52236def2 100644 --- a/intern/itasc/Object.hpp +++ b/intern/itasc/Object.hpp @@ -13,40 +13,62 @@ #include "kdl/frames.hpp" #include -namespace iTaSC{ +namespace iTaSC { class WorldObject; class Object { -public: - enum ObjectType {Controlled, UnControlled}; - static WorldObject world; + 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(){}; + private: + ObjectType m_type; - virtual int addEndEffector(const std::string& /*name*/){return 0;}; - virtual bool finalize(){return true;}; - virtual const KDL::Frame& getPose(const unsigned int end_effector=0){ - (void)end_effector; - 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;}; + 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 bool finalize() + { + return true; + }; + virtual const KDL::Frame &getPose(const unsigned int end_effector = 0) + { + (void)end_effector; + 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; + }; }; -} +} // namespace iTaSC #endif /* OBJECT_HPP_ */ diff --git a/intern/itasc/Scene.cpp b/intern/itasc/Scene.cpp index d9ebd9f3464..b84450b72da 100644 --- a/intern/itasc/Scene.cpp +++ b/intern/itasc/Scene.cpp @@ -19,12 +19,8 @@ class SceneLock : public ControlledObject::JointLockCallback { Range m_qrange; public: - SceneLock(Scene *scene) : m_scene(scene), m_qrange(0, 0) - { - } - virtual ~SceneLock() - { - } + SceneLock(Scene *scene) : m_scene(scene), m_qrange(0, 0) {} + virtual ~SceneLock() {} void setRange(Range &range) { @@ -375,7 +371,8 @@ bool Scene::update(double timestamp, } } if (os->object->getType() == Object::UnControlled && - ((UncontrolledObject *)os->object)->getNrOfCoordinates() != 0) { + ((UncontrolledObject *)os->object)->getNrOfCoordinates() != 0) + { ((UncontrolledObject *)(os->object))->updateCoordinates(ts); if (!ts.substep) { // velocity of uncontrolled object remains constant during substepping @@ -391,7 +388,8 @@ bool Scene::update(double timestamp, Object_struct *ob2 = cs->object2->second; if (ob1->base->updated() || ob1->object->updated() || ob2->base->updated() || - ob2->object->updated()) { + ob2->object->updated()) + { // the object from which the constraint depends have changed position // recompute the constraint pose getConstraintPose(cs->task, cs, external_pose); @@ -440,7 +438,8 @@ bool Scene::update(double timestamp, } } else if (ob1->object->getType() == Object::UnControlled && - ((UncontrolledObject *)ob1->object)->getNrOfCoordinates() != 0) { + ((UncontrolledObject *)ob1->object)->getNrOfCoordinates() != 0) + { // object1 is uncontrolled moving object project(m_Ju, cs->featurerange, @@ -479,7 +478,8 @@ bool Scene::update(double timestamp, } } else if (ob2->object->getType() == Object::UnControlled && - ((UncontrolledObject *)ob2->object)->getNrOfCoordinates() != 0) { + ((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); diff --git a/intern/itasc/Scene.hpp b/intern/itasc/Scene.hpp index 15b03f6221c..90f2d53fc36 100644 --- a/intern/itasc/Scene.hpp +++ b/intern/itasc/Scene.hpp @@ -11,9 +11,9 @@ #include "eigen_types.hpp" -#include "WorldObject.hpp" #include "ConstraintSet.hpp" #include "Solver.hpp" +#include "WorldObject.hpp" #include @@ -22,84 +22,110 @@ namespace iTaSC { class SceneLock; class Scene { - friend class SceneLock; -public: - enum SceneParam { - MIN_TIMESTEP = 0, - MAX_TIMESTEP, + friend class SceneLock; - COUNT - }; + public: + enum SceneParam { + MIN_TIMESTEP = 0, + MAX_TIMESTEP, + COUNT + }; - Scene(); - virtual ~Scene(); + 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); + 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 + 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 m_ytask; + 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 m_ytask; - Solver* m_solver; - Cache* m_cache; + 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 - 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 ObjectMap; - 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 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 ConstraintMap; - 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 ConstraintMap; + ObjectMap objects; + ConstraintMap constraints; - ObjectMap objects; - ConstraintMap constraints; - - static bool getConstraintPose(ConstraintSet* constraint, void *_param, KDL::Frame& _pose); + static bool getConstraintPose(ConstraintSet *constraint, void *_param, KDL::Frame &_pose); }; -} +} // namespace iTaSC #endif /* SCENE_HPP_ */ diff --git a/intern/itasc/Solver.hpp b/intern/itasc/Solver.hpp index 84a1e422394..318194e79c7 100644 --- a/intern/itasc/Solver.hpp +++ b/intern/itasc/Solver.hpp @@ -9,26 +9,27 @@ #ifndef SOLVER_HPP_ #define SOLVER_HPP_ -#include #include "eigen_types.hpp" +#include -namespace iTaSC{ +namespace iTaSC { -class Solver{ -public: - enum SolverParam { - DLS_QMAX = 0, - DLS_LAMBDA_MAX, - DLS_EPSILON - }; - virtual ~Solver(){}; +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& 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; + // 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 &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; }; -} +} // namespace iTaSC #endif /* SOLVER_HPP_ */ diff --git a/intern/itasc/UncontrolledObject.cpp b/intern/itasc/UncontrolledObject.cpp index e808e70b357..761f15aa1d3 100644 --- a/intern/itasc/UncontrolledObject.cpp +++ b/intern/itasc/UncontrolledObject.cpp @@ -8,37 +8,30 @@ #include "UncontrolledObject.hpp" -namespace iTaSC{ +namespace iTaSC { -UncontrolledObject::UncontrolledObject():Object(UnControlled), - m_nu(0), m_nf(0), m_xudot() -{ -} +UncontrolledObject::UncontrolledObject() : Object(UnControlled), m_nu(0), m_nf(0), m_xudot() {} -UncontrolledObject::~UncontrolledObject() -{ -} +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)); + 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 +const e_matrix &UncontrolledObject::getJu(unsigned int frameIndex) const { - assert (m_nu > 0); - return m_JuArray[(frameIndex>m_nf)?m_nf:frameIndex]; + assert(m_nu > 0); + return m_JuArray[(frameIndex > m_nf) ? m_nf : frameIndex]; } - - -} +} // namespace iTaSC diff --git a/intern/itasc/UncontrolledObject.hpp b/intern/itasc/UncontrolledObject.hpp index d08c391e34e..059f442df64 100644 --- a/intern/itasc/UncontrolledObject.hpp +++ b/intern/itasc/UncontrolledObject.hpp @@ -12,27 +12,35 @@ #include "eigen_types.hpp" #include "Object.hpp" -namespace iTaSC{ +namespace iTaSC { -class UncontrolledObject: public Object { -protected: - unsigned int m_nu, m_nf; - e_vector m_xudot; - std::vector m_JuArray; +class UncontrolledObject : public Object { + protected: + unsigned int m_nu, m_nf; + e_vector m_xudot; + std::vector 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;}; + 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 ×tamp) = 0; + virtual const unsigned int getNrOfCoordinates() + { + return m_nu; + }; + virtual const unsigned int getNrOfFrames() + { + return m_nf; + }; }; -} +} // namespace iTaSC #endif /* UNCONTROLLEDOBJECT_H_ */ diff --git a/intern/itasc/WDLSSolver.cpp b/intern/itasc/WDLSSolver.cpp index b76558a9d34..9ba16e2b13c 100644 --- a/intern/itasc/WDLSSolver.cpp +++ b/intern/itasc/WDLSSolver.cpp @@ -11,92 +11,100 @@ namespace iTaSC { -WDLSSolver::WDLSSolver() : m_lambda(0.5), m_epsilon(0.1) +WDLSSolver::WDLSSolver() : m_lambda(0.5), m_epsilon(0.1) { - // maximum joint velocity - m_qmax = 50.0; + // maximum joint velocity + m_qmax = 50.0; } -WDLSSolver::~WDLSSolver() { -} +WDLSSolver::~WDLSSolver() {} -bool WDLSSolver::init(unsigned int nq, unsigned int nc, const std::vector& gc) +bool WDLSSolver::init(unsigned int nq, unsigned int nc, const std::vector &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; + 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) +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 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).array().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; + // Wy*ydot + m_Wy_ydot = Wy.array() * ydot.array(); + // 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).array().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; } -} +} // namespace iTaSC diff --git a/intern/itasc/WDLSSolver.hpp b/intern/itasc/WDLSSolver.hpp index 8474d2daa16..1d88be46374 100644 --- a/intern/itasc/WDLSSolver.hpp +++ b/intern/itasc/WDLSSolver.hpp @@ -13,37 +13,43 @@ 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(); +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; - virtual bool init(unsigned int nq, unsigned int nc, const std::vector& 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; - } - } + public: + WDLSSolver(); + virtual ~WDLSSolver(); + + virtual bool init(unsigned int nq, unsigned int nc, const std::vector &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; + } + } }; -} +} // namespace iTaSC #endif /* WDLSSOLVER_HPP_ */ diff --git a/intern/itasc/WSDLSSolver.cpp b/intern/itasc/WSDLSSolver.cpp index 8074f0ae1ac..a4f136da9e3 100644 --- a/intern/itasc/WSDLSSolver.cpp +++ b/intern/itasc/WSDLSSolver.cpp @@ -12,128 +12,136 @@ namespace iTaSC { -WSDLSSolver::WSDLSSolver() : - m_ns(0), m_nc(0), m_nq(0) +WSDLSSolver::WSDLSSolver() : m_ns(0), m_nc(0), m_nq(0) { - // default maximum speed: 50 rad/s - m_qmax = 50.0; + // default maximum speed: 50 rad/s + m_qmax = 50.0; } -WSDLSSolver::~WSDLSSolver() { -} +WSDLSSolver::~WSDLSSolver() {} -bool WSDLSSolver::init(unsigned int _nq, unsigned int _nc, const std::vector& gc) +bool WSDLSSolver::init(unsigned int _nq, unsigned int _nc, const std::vector &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; + 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) +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; + unsigned int i, j, l; + e_scalar N, M; - // Create the Weighted jacobian - m_AWq.noalias() = A*Wq; - for (i=0; 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 _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; + m_Wy_ydot = Wy.array() * ydot.array(); + m_WqV.noalias() = Wq * m_V; + 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).array().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; } -} +} // namespace iTaSC diff --git a/intern/itasc/WSDLSSolver.hpp b/intern/itasc/WSDLSSolver.hpp index 07acb11e85c..b806747e742 100644 --- a/intern/itasc/WSDLSSolver.hpp +++ b/intern/itasc/WSDLSSolver.hpp @@ -13,32 +13,38 @@ 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 m_ytask; - e_scalar m_qmax; - unsigned int m_ns, m_nc, m_nq; - bool m_transpose; -public: - WSDLSSolver(); - virtual ~WSDLSSolver(); +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 m_ytask; + e_scalar m_qmax; + unsigned int m_ns, m_nc, m_nq; + bool m_transpose; - virtual bool init(unsigned int _nq, unsigned int _nc, const std::vector& 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; - } - } + public: + WSDLSSolver(); + virtual ~WSDLSSolver(); + + virtual bool init(unsigned int _nq, unsigned int _nc, const std::vector &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; + } + } }; -} +} // namespace iTaSC #endif /* WSDLSSOLVER_HPP_ */ diff --git a/intern/itasc/WorldObject.cpp b/intern/itasc/WorldObject.cpp index 7e547b3bad5..aedb5f3290c 100644 --- a/intern/itasc/WorldObject.cpp +++ b/intern/itasc/WorldObject.cpp @@ -8,20 +8,17 @@ #include "WorldObject.hpp" -namespace iTaSC{ +namespace iTaSC { /* special singleton to be used as base for uncontrolled object */ WorldObject Object::world; -WorldObject::WorldObject():UncontrolledObject() +WorldObject::WorldObject() : UncontrolledObject() { - initialize(0,1); - m_internalPose = Frame::Identity(); + initialize(0, 1); + m_internalPose = Frame::Identity(); } -WorldObject::~WorldObject() -{ -} +WorldObject::~WorldObject() {} - -} +} // namespace iTaSC diff --git a/intern/itasc/WorldObject.hpp b/intern/itasc/WorldObject.hpp index 77209e5ee2b..bf118f2c46d 100644 --- a/intern/itasc/WorldObject.hpp +++ b/intern/itasc/WorldObject.hpp @@ -10,22 +10,22 @@ #define WORLDOBJECT_HPP_ #include "UncontrolledObject.hpp" -namespace iTaSC{ +namespace iTaSC { -class WorldObject: public UncontrolledObject { -public: - WorldObject(); - virtual ~WorldObject(); +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() {} + 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() {} }; -} +} // namespace iTaSC #endif /* WORLDOBJECT_H_ */ diff --git a/intern/itasc/eigen_types.cpp b/intern/itasc/eigen_types.cpp index 7c9273904cc..03b2879e622 100644 --- a/intern/itasc/eigen_types.cpp +++ b/intern/itasc/eigen_types.cpp @@ -8,6 +8,4 @@ #include "eigen_types.hpp" -const KDL::Frame iTaSC::F_identity(Rotation::Identity(),Vector::Zero()); - - +const KDL::Frame iTaSC::F_identity(Rotation::Identity(), Vector::Zero()); diff --git a/intern/itasc/eigen_types.hpp b/intern/itasc/eigen_types.hpp index 5c6dab33b71..0559565921e 100644 --- a/intern/itasc/eigen_types.hpp +++ b/intern/itasc/eigen_types.hpp @@ -9,29 +9,28 @@ #ifndef EIGEN_TYPES_HPP_ #define EIGEN_TYPES_HPP_ -#include -#include "kdl/frames.hpp" -#include "kdl/tree.hpp" #include "kdl/chain.hpp" +#include "kdl/frames.hpp" #include "kdl/jacobian.hpp" #include "kdl/jntarray.hpp" +#include "kdl/tree.hpp" +#include +namespace iTaSC { -namespace iTaSC{ - -using KDL::Twist; +using KDL::Chain; using KDL::Frame; -using KDL::Joint; using KDL::Inertia; +using KDL::Jacobian; +using KDL::JntArray; +using KDL::Joint; +using KDL::Rotation; +using KDL::Segment; using KDL::SegmentMap; using KDL::Tree; -using KDL::JntArray; -using KDL::Jacobian; -using KDL::Segment; -using KDL::Rotation; +using KDL::Twist; using KDL::Vector; using KDL::Vector2; -using KDL::Chain; extern const Frame F_identity; @@ -39,47 +38,58 @@ extern const Frame F_identity; #define e_vector Eigen::Matrix #define e_zero_vector Eigen::Matrix::Zero #define e_matrix Eigen::Matrix -#define e_matrix6 Eigen::Matrix +#define e_matrix6 Eigen::Matrix #define e_identity_matrix Eigen::Matrix::Identity #define e_scalar_vector Eigen::Matrix::Constant #define e_zero_matrix Eigen::Matrix::Zero #define e_random_matrix Eigen::Matrix::Random -#define e_vector6 Eigen::Matrix -#define e_vector3 Eigen::Matrix +#define e_vector6 Eigen::Matrix +#define e_vector3 Eigen::Matrix 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; } + 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 inline Eigen::Block project(MatrixType& m, Range r) +template inline Eigen::Block project(MatrixType &m, Range r) { - return Eigen::Block(m,r.start,0,r.count,1); + return Eigen::Block(m, r.start, 0, r.count, 1); } -template inline Eigen::Block project(MatrixType& m, Range r, Range c) +template +inline Eigen::Block project(MatrixType &m, Range r, Range c) { - return Eigen::Block(m,r.start,c.start,r.count,c.count); + return Eigen::Block(m, r.start, c.start, r.count, c.count); } -template inline static int changeBase(Eigen::MatrixBase& J, const Frame& T) { +template +inline static int changeBase(Eigen::MatrixBase &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; + 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; } -} +} // namespace iTaSC #endif /* UBLAS_TYPES_HPP_ */ diff --git a/intern/itasc/kdl/chain.cpp b/intern/itasc/kdl/chain.cpp index 8137ade1847..61c5701e82a 100644 --- a/intern/itasc/kdl/chain.cpp +++ b/intern/itasc/kdl/chain.cpp @@ -25,54 +25,44 @@ #include "chain.hpp" namespace KDL { - using namespace std; +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;iaddSegment(in.getSegment(i)); - } - - Chain& Chain::operator=(const Chain& arg) - { - nrOfJoints=0; - nrOfSegments=0; - segments.resize(0); - for(unsigned int i=0;iaddSegment(chain.getSegment(i)); - } - - const Segment& Chain::getSegment(unsigned int nr) const - { - return segments[nr]; - } - - Chain::~Chain() - { - } +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() {} + +} // namespace KDL diff --git a/intern/itasc/kdl/chain.hpp b/intern/itasc/kdl/chain.hpp index 81c606b73c0..1ec6f45f3dc 100644 --- a/intern/itasc/kdl/chain.hpp +++ b/intern/itasc/kdl/chain.hpp @@ -23,75 +23,80 @@ #define KDL_CHAIN_HPP #include "segment.hpp" -#include #include +#include namespace KDL { - /** - * \brief This class encapsulates a serial kinematic - * interconnection structure. It is build out of segments. - * - * @ingroup KinematicFamily - */ - class Chain { - private: - // Eigen allocator is needed for alignment of Eigen data types - std::vector > 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); +/** + * \brief This class encapsulates a serial kinematic + * interconnection structure. It is build out of segments. + * + * @ingroup KinematicFamily + */ +class Chain { + private: + // Eigen allocator is needed for alignment of Eigen data types + std::vector> segments; + unsigned int nrOfJoints; + unsigned int nrOfSegments; - /** - * Adds a new segment to the end of the chain. - * - * @param segment The segment to add - */ - void addSegment(const Segment& segment); - /** - * Adds a complete chain to the end of the chain - * The added chain is copied. - * - * @param chain The chain to add - */ - void addChain(const Chain& chain); + public: + /** + * The constructor of a chain, a new chain is always empty. + * + */ + Chain(); + Chain(const Chain &in); + Chain &operator=(const Chain &arg); - /** - * Request the total number of joints in the chain.\n - * Important: 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;}; + /** + * Adds a new segment to the end of the chain. + * + * @param segment The segment to add + */ + void addSegment(const Segment &segment); + /** + * Adds a complete chain to the end of the chain + * The added chain is copied. + * + * @param chain The chain to add + */ + void addChain(const Chain &chain); - /** - * 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; + /** + * Request the total number of joints in the chain.\n + * Important: 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; + }; - virtual ~Chain(); - }; + /** + * 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 +} // end of namespace KDL #endif diff --git a/intern/itasc/kdl/chainfksolver.hpp b/intern/itasc/kdl/chainfksolver.hpp index fa6f625ee9d..88d7f4b653f 100644 --- a/intern/itasc/kdl/chainfksolver.hpp +++ b/intern/itasc/kdl/chainfksolver.hpp @@ -23,85 +23,84 @@ #define KDL_CHAIN_FKSOLVER_HPP #include "chain.hpp" -#include "framevel.hpp" #include "frameacc.hpp" +#include "framevel.hpp" #include "jntarray.hpp" -#include "jntarrayvel.hpp" #include "jntarrayacc.hpp" +#include "jntarrayvel.hpp" namespace KDL { - /** - * \brief This abstract class encapsulates a - * solver for the forward position kinematics for a KDL::Chain. - * - * @ingroup KinematicFamily - */ +/** + * \brief This abstract 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(){}; - }; +// 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 abstract 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; +/** + * \brief This abstract 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(){}; - }; + virtual ~ChainFkSolverVel(){}; +}; - /** - * \brief This abstract class encapsulates a solver - * for the forward acceleration kinematics for a KDL::Chain. - * - * @ingroup KinematicFamily - */ +/** + * \brief This abstract 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; +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; - }; + virtual ~ChainFkSolverAcc() = 0; +}; - -}//end of namespace KDL +} // end of namespace KDL #endif diff --git a/intern/itasc/kdl/chainfksolverpos_recursive.cpp b/intern/itasc/kdl/chainfksolverpos_recursive.cpp index d5149256c0d..40cb4b90377 100644 --- a/intern/itasc/kdl/chainfksolverpos_recursive.cpp +++ b/intern/itasc/kdl/chainfksolverpos_recursive.cpp @@ -28,37 +28,30 @@ namespace KDL { - ChainFkSolverPos_recursive::ChainFkSolverPos_recursive(const Chain& _chain): - chain(_chain) - { +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(); } - - 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= 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= 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; } - +} // namespace KDL diff --git a/intern/itasc/kdl/chainjnttojacsolver.hpp b/intern/itasc/kdl/chainjnttojacsolver.hpp index 98f9d06ee0d..f00422cfd0b 100644 --- a/intern/itasc/kdl/chainjnttojacsolver.hpp +++ b/intern/itasc/kdl/chainjnttojacsolver.hpp @@ -22,44 +22,41 @@ #ifndef KDL_CHAINJNTTOJACSOLVER_HPP #define KDL_CHAINJNTTOJACSOLVER_HPP +#include "chain.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. - * - * - */ +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); +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; - }; -} + private: + const Chain chain; + Twist t_local; + Frame T_total; +}; +} // namespace KDL #endif - diff --git a/intern/itasc/kdl/frameacc.cpp b/intern/itasc/kdl/frameacc.cpp index 7c294aeb268..342d753043e 100644 --- a/intern/itasc/kdl/frameacc.cpp +++ b/intern/itasc/kdl/frameacc.cpp @@ -14,14 +14,12 @@ * $Name: $ ****************************************************************************/ - #include "frameacc.hpp" namespace KDL { #ifndef KDL_INLINE - #include "frameacc.inl" +# include "frameacc.inl" #endif -} - +} // namespace KDL diff --git a/intern/itasc/kdl/frameacc.hpp b/intern/itasc/kdl/frameacc.hpp index 383cfa1e937..271ccbc681e 100644 --- a/intern/itasc/kdl/frameacc.hpp +++ b/intern/itasc/kdl/frameacc.hpp @@ -27,240 +27,210 @@ #ifndef RRFRAMES_H #define RRFRAMES_H - -#include "utilities/rall2d.h" #include "frames.hpp" - - +#include "utilities/rall2d.h" namespace KDL { class TwistAcc; -typedef Rall2d doubleAcc; +typedef Rall2d 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); -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 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); - IMETHOD friend bool Equal(const Vector& r1,const VectorAcc& r2,double eps); - IMETHOD friend bool Equal(const VectorAcc& r1,const Vector& r2,double eps); - 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); + IMETHOD friend bool Equal(const VectorAcc &r1, const VectorAcc &r2, double eps); + IMETHOD friend bool Equal(const Vector &r1, const VectorAcc &r2, double eps); + IMETHOD friend bool Equal(const VectorAcc &r1, const Vector &r2, double eps); + 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); -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; + // 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 ! - // 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); - IMETHOD friend bool Equal(const Rotation& r1,const RotationAcc& r2,double eps); - IMETHOD friend bool Equal(const RotationAcc& r1,const Rotation& r2,double eps); - 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; + 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); + IMETHOD friend bool Equal(const Rotation &r1, const RotationAcc &r2, double eps); + IMETHOD friend bool Equal(const RotationAcc &r1, const Rotation &r2, double eps); + 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); + IMETHOD friend bool Equal(const Frame &r1, const FrameAcc &r2, double eps); + IMETHOD friend bool Equal(const FrameAcc &r1, const Frame &r2, double eps); - -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); - IMETHOD friend bool Equal(const Frame& r1,const FrameAcc& r2,double eps); - IMETHOD friend bool Equal(const FrameAcc& r1,const Frame& r2,double eps); - - 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; + 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(); -//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: + IMETHOD void ReverseSign(); - TwistAcc():vel(),rot() {}; - TwistAcc(const VectorAcc& _vel,const VectorAcc& _rot):vel(_vel),rot(_rot) {}; + 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 TwistAcc& operator-=(const TwistAcc& arg); - IMETHOD TwistAcc& operator+=(const TwistAcc& arg); + IMETHOD friend bool Equal(const TwistAcc &a, const TwistAcc &b, double eps); + IMETHOD friend bool Equal(const Twist &a, const TwistAcc &b, double eps); + IMETHOD friend bool Equal(const TwistAcc &a, const Twist &b, double eps); - 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); - IMETHOD friend bool Equal(const Twist& a,const TwistAcc& b,double eps); - IMETHOD friend bool Equal(const TwistAcc& a,const Twist& b,double eps); - - - IMETHOD Twist GetTwist() const; - IMETHOD Twist GetTwistDot() const; - - friend class RotationAcc; - friend class FrameAcc; + IMETHOD Twist GetTwist() const; + IMETHOD Twist GetTwistDot() const; + friend class RotationAcc; + friend class FrameAcc; }; - -IMETHOD bool Equal(const VectorAcc&, const VectorAcc&, double = epsilon); -IMETHOD bool Equal(const Vector&, const VectorAcc&, double = epsilon); -IMETHOD bool Equal(const VectorAcc&, const Vector&, double = epsilon); -IMETHOD bool Equal(const RotationAcc&, const RotationAcc&, double = epsilon); -IMETHOD bool Equal(const Rotation&, const RotationAcc&, double = epsilon); -IMETHOD bool Equal(const RotationAcc&, const Rotation&, double = epsilon); -IMETHOD bool Equal(const FrameAcc&, const FrameAcc&, double = epsilon); -IMETHOD bool Equal(const Frame&, const FrameAcc&, double = epsilon); -IMETHOD bool Equal(const FrameAcc&, const Frame&, double = epsilon); -IMETHOD bool Equal(const TwistAcc&, const TwistAcc&, double = epsilon); -IMETHOD bool Equal(const Twist&, const TwistAcc&, double = epsilon); -IMETHOD bool Equal(const TwistAcc&, const Twist&, double = epsilon); - +IMETHOD bool Equal(const VectorAcc &, const VectorAcc &, double = epsilon); +IMETHOD bool Equal(const Vector &, const VectorAcc &, double = epsilon); +IMETHOD bool Equal(const VectorAcc &, const Vector &, double = epsilon); +IMETHOD bool Equal(const RotationAcc &, const RotationAcc &, double = epsilon); +IMETHOD bool Equal(const Rotation &, const RotationAcc &, double = epsilon); +IMETHOD bool Equal(const RotationAcc &, const Rotation &, double = epsilon); +IMETHOD bool Equal(const FrameAcc &, const FrameAcc &, double = epsilon); +IMETHOD bool Equal(const Frame &, const FrameAcc &, double = epsilon); +IMETHOD bool Equal(const FrameAcc &, const Frame &, double = epsilon); +IMETHOD bool Equal(const TwistAcc &, const TwistAcc &, double = epsilon); +IMETHOD bool Equal(const Twist &, const TwistAcc &, double = epsilon); +IMETHOD bool Equal(const TwistAcc &, const Twist &, double = epsilon); #ifdef KDL_INLINE -#include "frameacc.inl" +# include "frameacc.inl" #endif -} - - - - +} // namespace KDL #endif diff --git a/intern/itasc/kdl/frames.cpp b/intern/itasc/kdl/frames.cpp index 747a54909e9..9c76215bf49 100644 --- a/intern/itasc/kdl/frames.cpp +++ b/intern/itasc/kdl/frames.cpp @@ -33,314 +33,333 @@ namespace KDL { #ifndef KDL_INLINE -#include "frames.inl" +# include "frames.inl" #endif -void Frame::Make4x4(double * d) +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; + 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) +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 ) - ); + 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) +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 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)); - } + 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; - } +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]); + double tmp1; + double tmp2; + tmp1 = fabs(data[0]); + tmp2 = fabs(data[1]); + if (tmp1 >= tmp2) { + tmp2 = fabs(data[2]); 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])); - } + 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; - } +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) ); +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; + 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) +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] - ); - + 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); - } +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) - ); +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; +// 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; - } + 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; + // [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 @@ -351,42 +370,37 @@ Vector2 Rotation::GetXZRot() const * /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; +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) { +bool operator==(const Rotation &a, const Rotation &b) +{ #ifdef KDL_USE_EQUAL - return Equal(a,b); + 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] ); + 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 } -} +} // namespace KDL diff --git a/intern/itasc/kdl/frames.hpp b/intern/itasc/kdl/frames.hpp index 662eb3e857a..5bb94eb88b6 100644 --- a/intern/itasc/kdl/frames.hpp +++ b/intern/itasc/kdl/frames.hpp @@ -115,7 +115,6 @@ #ifndef KDL_FRAMES_H #define KDL_FRAMES_H - #include "utilities/kdl-config.h" #include "utilities/utility.h" @@ -123,8 +122,6 @@ namespace KDL { - - class Vector; class Rotation; class Frame; @@ -134,133 +131,128 @@ 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;} +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 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(double *xyz); - //! Constructs a vector out of an array of three values x, y and z - inline Vector(float* 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); + //! Assignment operator. The normal copy by value semantics. + inline Vector(const Vector &arg); - //! store vector components in array - inline void GetValue(double* xyz) const; + //! 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); + //! 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) const; - //! Access to elements, range checked when NDEBUG is not set, from 0..2 - inline double& operator() (int index); + //! 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) const + double operator[](int index) const + { + return this->operator()(index); + } - //! Equivalent to double& operator()(int index) - double& operator[] ( int index ) - { - 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); + 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(); + //! 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); - //! 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); + //! Multiply by a scalar + inline Vector &operator*=(double 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 - //! Multiply by a scalar - inline Vector& operator *=(double arg); + 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); - //! 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 + //! To have a uniform operator to put an element to zero, for scalar values + //! and for objects. + inline friend void SetToZero(Vector &v); - 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); + //! @return a zero vector + inline static Vector Zero(); - //! To have a uniform operator to put an element to zero, for scalar values - //! and for objects. - inline friend void SetToZero(Vector& v); + /** 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 a zero vector - inline static Vector Zero(); + //! @return the norm of the vector + double Norm() const; - /** 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); + //! 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); - //! @return the norm of the vector - double Norm() 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 Vector &a, const Vector &b, double eps); + //! return a normalized vector + inline friend Vector Normalize(const Vector &a, double eps); + //! 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); - //! 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); - - //! return a normalized vector - inline friend Vector Normalize(const Vector& a, double eps); - - //! 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; + friend class Rotation; + friend class Frame; }; - inline Vector Normalize(const Vector&, double eps=epsilon); +inline Vector Normalize(const Vector &, double eps = epsilon); /** \brief represents rotations in 3 dimensional space. @@ -295,227 +287,234 @@ public: \par type Concrete implementation */ -class Rotation -{ -public: - double data[9]; +class Rotation { + public: + double data[9]; - inline Rotation() { - *this = 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 Rotation() + { + *this = 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 void setValue(float *oglmat); + inline void getValue(float *oglmat) const; - inline Rotation& operator=(const Rotation& arg); + 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; + //! 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); - //! Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set - inline double operator() (int i,int j) const; + //! 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); + friend Rotation operator*(const Rotation &lhs, const Rotation &rhs); - //! Sets the value of *this to its inverse. - inline void SetInverse(); + //! Sets the value of *this to its inverse. + inline void SetInverse(); - //! Gives back the inverse rotation matrix of *this. - inline Rotation Inverse() const; + //! 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()*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 Wrench Inverse(const Wrench &arg) const; - //! The same as R.Inverse()*arg but more efficient. - inline Twist Inverse(const Twist& 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(); + //! 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); -// = 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 too 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. It is not necessary to normalize rotaxis. - //! returns identity rotation matrix in the case that the norm of rotaxis - //! is too 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); - //! 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(); + // 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 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 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; + /** 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 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; - //! 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; - //! 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 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); + } - //! 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; + //! 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; - //! 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); - } + //! 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; - //! 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); - } + //! Access to the underlying unitvectors of the rotation matrix + inline Vector UnitX() const + { + return Vector(data[0], data[3], data[6]); + } - //! 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; + //! 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); + } - //! 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 UnitY() const + { + return Vector(data[1], data[4], data[7]); + } - //! 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 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 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 UnitZ() const + { + return Vector(data[2], data[5], data[8]); + } - //! 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 UnitZ(const Vector &X) + { + data[2] = X(0); + data[5] = X(1); + data[8] = X(2); + } - //! 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); - } + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval - //! Access to the underlying unitvectors of the rotation matrix - inline Vector UnitZ() const { - return Vector(data[2],data[5],data[8]); - } + //! 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); - //! 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 - - - //! 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; + friend class Frame; }; - bool operator==(const Rotation& a,const Rotation& b); - - +bool operator==(const Rotation &a, const Rotation &b); /** - \brief represents a frame transformation in 3D space (rotation + translation) + \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 @@ -524,150 +523,148 @@ public: 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: + Vector p; //!< origine of the Frame + Rotation M; //!< Orientation of the Frame -public: + public: + inline Frame(const Rotation &R, const Vector &V); - 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); - //! 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 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); - 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); - //! 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); + //! 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; - //! 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; - // = 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 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 Wrench Inverse(const Wrench& arg) const; + //! The same as p2=R.Inverse()*p but more efficient. + inline Twist Inverse(const Twist &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); - //! 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 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 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; - //! 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); - //! 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(); - //! @return the identity transformation Frame(Rotation::Identity(),Vector::Zero()). - inline static Frame Identity(); + //! The twist is expressed wrt the current + //! frame. This frame is integrated into an updated frame with + //! . Very simple first order integration rule. + inline void Integrate(const Twist &t_this, double frequency); - //! The twist is expressed wrt the current - //! frame. This frame is integrated into an updated frame with - //! . 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_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. - // 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); - 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); - - //! 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); - - //! 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); + //! 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); }; /** @@ -675,76 +672,72 @@ public: * * 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: + 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(){}; - //! 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){}; - 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); - 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; - //! 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 ) const - { - return this->operator() ( index ); - } + double &operator[](int index) + { + 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); - 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(); - //! @return a zero Twist : Twist(Vector::Zero(),Vector::Zero()) - static inline Twist Zero(); + //! 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; - //! Reverses the sign of the twist - inline void ReverseSign(); + //! 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); - //! 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); - - //! 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; + //! 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; }; /** @@ -753,7 +746,7 @@ public: * 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: @@ -777,20 +770,21 @@ public: double operator[] ( int index ) const { - return this->operator() ( index ); - } + return this->operator() ( index ); + } double& operator[] ( int index ) { - return this->operator() ( 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 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); @@ -813,11 +807,12 @@ public: //! 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); + inline friend bool Equal(const AccelerationTwist& a,const AccelerationTwist& b,double +eps=epsilon); - //! The literal equality operator==(), also identical. + //! The literal equality operator==(), also identical. inline friend bool operator==(const AccelerationTwist& a,const AccelerationTwist& b); - //! The literal inequality operator!=(). + //! The literal inequality operator!=(). inline friend bool operator!=(const AccelerationTwist& a,const AccelerationTwist& b); // = Friends @@ -831,278 +826,270 @@ public: * * 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: +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){}; - //! 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); -// = 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) - 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; - //! 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 ) const - { - return this->operator() ( index ); - } + double &operator[](int index) + { + 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); - //! 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); - 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); - //! 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); - //! 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(); - //! @return a zero Wrench - static inline Wrench Zero(); + //! Reverses the sign of the current Wrench + inline void ReverseSign(); - //! 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; - //! 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); - - //! 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; + //! 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); + //! 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); +class Vector2 { + double data[2]; - inline Vector2& operator = ( const Vector2& arg); + 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); - //! Access to elements, range checked when NDEBUG is not set, from 0..1 - inline double operator()(int index) const; + inline Vector2 &operator=(const Vector2 &arg); - //! Access to elements, range checked when NDEBUG is not set, from 0..1 - inline double& operator() (int index); + //! Access to elements, range checked when NDEBUG is not set, from 0..1 + inline double operator()(int index) const; - //! store vector components in array - inline void GetValue(double* xy) const; + //! Access to elements, range checked when NDEBUG is not set, from 0..1 + inline double &operator()(int index); - inline void ReverseSign(); - inline Vector2& operator-=(const Vector2& arg); - inline Vector2& operator +=(const Vector2& arg); + //! 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); + 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(); + //! @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); + /** 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; + //! @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 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 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 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); + //! 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); - //! 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); - - friend class Rotation2; + 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;} +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)) {} + explicit Rotation2(double angle_rad) : s(sin(angle_rad)), c(cos(angle_rad)) {} - Rotation2(double ca,double sa):s(sa),c(ca){} + 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 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 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 SetInverse(); + inline Rotation2 Inverse() const; + inline Vector2 Inverse(const Vector2 &v) const; - inline void SetIdentity(); - inline static Rotation2 Identity(); + 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 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); - //! 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; - //! 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); + //! 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); }; //! 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 +class Frame2 { + public: + Vector2 p; //!< origine of the Frame + Rotation2 M; //!< Orientation of the Frame -public: + 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); - 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 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; - //! 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); + 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); }; -inline bool Equal(const Vector&, const Vector&, double = epsilon); - bool Equal(const Rotation&, const Rotation&, double = epsilon); -inline bool Equal(const Frame&, const Frame&, double = epsilon); -inline bool Equal(const Twist&, const Twist&, double = epsilon); -inline bool Equal(const Wrench&, const Wrench&, double = epsilon); -inline bool Equal(const Vector2&, const Vector2&, double = epsilon); -inline bool Equal(const Rotation2&, const Rotation2&, double = epsilon); -inline bool Equal(const Frame2&, const Frame2&, double = 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); +inline bool Equal(const Vector &, const Vector &, double = epsilon); +bool Equal(const Rotation &, const Rotation &, double = epsilon); +inline bool Equal(const Frame &, const Frame &, double = epsilon); +inline bool Equal(const Twist &, const Twist &, double = epsilon); +inline bool Equal(const Wrench &, const Wrench &, double = epsilon); +inline bool Equal(const Vector2 &, const Vector2 &, double = epsilon); +inline bool Equal(const Rotation2 &, const Rotation2 &, double = epsilon); +inline bool Equal(const Frame2 &, const Frame2 &, double = 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" +// #include "rotation.inl" +// #include "frame.inl" +// #include "twist.inl" +// #include "vector2.inl" +// #include "rotation2.inl" +// #include "frame2.inl" +# include "frames.inl" #endif - - -} - +} // namespace KDL #endif diff --git a/intern/itasc/kdl/frames_io.cpp b/intern/itasc/kdl/frames_io.cpp index 782524ce77c..198cb631ffa 100644 --- a/intern/itasc/kdl/frames_io.cpp +++ b/intern/itasc/kdl/frames_io.cpp @@ -29,285 +29,290 @@ * * ***************************************************************************/ +#include "frames_io.hpp" +#include "frames.hpp" #include "utilities/error.h" #include "utilities/error_stack.h" -#include "frames.hpp" -#include "frames_io.hpp" -#include #include -#include #include +#include +#include namespace KDL { - -std::ostream& operator << (std::ostream& os,const Vector& v) { - os << "[" << std::setw(KDL_FRAME_WIDTH) << v(0) << "," << std::setw(KDL_FRAME_WIDTH)<> 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) +std::ostream &operator<<(std::ostream &os, const Rotation2 &R) { - os << T.M << T.p; - return os; + os << "[" << R.GetRot() * rad2deg << "]"; + 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 (storage[0]=='\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::ostream &operator<<(std::ostream &os, const Frame2 &T) +{ + os << T.M << T.p; + return os; } -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 (storage[0]=='\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 (storage[0]=='\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,'['); +std::istream &operator>>(std::istream &is, Vector &v) +{ + IOTrace("Stream input Vector (vector or ZERO)"); + char storage[10]; + EatWord(is, "[]", storage, 10); + if (storage[0] == '\0') { + Eat(is, '['); is >> v(0); - Eat(is,','); + Eat(is, ','); is >> v(1); - EatEnd(is,']'); + Eat(is, ','); + is >> v(2); + 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,']'); + } + if (strcmp(storage, "ZERO") == 0) { + v = Vector::Zero(); IOTracePop(); return is; + } + throw Error_Frame_Vector_Unexpected_id(); } -std::istream& operator >> (std::istream& is,Frame2& T) -{ IOTrace("Stream input Frame2"); + +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 (storage[0] == '\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 (storage[0] == '\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; } -} // namespace Frame +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 KDL diff --git a/intern/itasc/kdl/frames_io.hpp b/intern/itasc/kdl/frames_io.hpp index 4f47fe178d8..59d9f09f312 100644 --- a/intern/itasc/kdl/frames_io.hpp +++ b/intern/itasc/kdl/frames_io.hpp @@ -76,39 +76,35 @@ #ifndef FRAMES_IO_H #define FRAMES_IO_H -#include "utilities/utility_io.h" #include "frames.hpp" -#include "jntarray.hpp" #include "jacobian.hpp" +#include "jntarray.hpp" +#include "utilities/utility_io.h" namespace KDL { - //! width to be used when printing variables out with frames_io.h - //! global variable, can be changed. +//! 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); - // 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); - - - 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 +} // namespace KDL #endif diff --git a/intern/itasc/kdl/framevel.cpp b/intern/itasc/kdl/framevel.cpp index d86d9ad2ef6..02ae3dc034c 100644 --- a/intern/itasc/kdl/framevel.cpp +++ b/intern/itasc/kdl/framevel.cpp @@ -14,15 +14,12 @@ * $Name: $ ****************************************************************************/ - #include "framevel.hpp" namespace KDL { #ifndef KDL_INLINE - #include "framevel.inl" +# include "framevel.inl" #endif - - -} +} // namespace KDL diff --git a/intern/itasc/kdl/framevel.hpp b/intern/itasc/kdl/framevel.hpp index 13043654439..79339f32fa0 100644 --- a/intern/itasc/kdl/framevel.hpp +++ b/intern/itasc/kdl/framevel.hpp @@ -3,10 +3,9 @@ * 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. - * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * 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. Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 @@ -21,41 +20,42 @@ #ifndef KDL_FRAMEVEL_H #define KDL_FRAMEVEL_H -#include "utilities/utility.h" #include "utilities/rall1d.h" #include "utilities/traits.h" +#include "utilities/utility.h" #include "frames.hpp" - - namespace KDL { typedef Rall1d 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 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 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 random(doubleVel &F) +{ + random(F.t); + random(F.grad); } -IMETHOD void posrandom(doubleVel& F) { - posrandom(F.t); - posrandom(F.grad); +IMETHOD void posrandom(doubleVel &F) +{ + posrandom(F.t); + posrandom(F.grad); } -} +} // namespace KDL -template <> -struct Traits { - typedef double valueType; - typedef KDL::doubleVel derivType; +template<> struct Traits { + typedef double valueType; + typedef KDL::doubleVel derivType; }; namespace KDL { @@ -71,113 +71,117 @@ class VectorVel // = 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()) {} + 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;} + 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 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 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); - IMETHOD friend bool Equal(const Vector& r1,const VectorVel& r2,double eps); - IMETHOD friend bool Equal(const VectorVel& r1,const Vector& r2,double eps); - 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); + IMETHOD friend bool Equal(const VectorVel &r1, const VectorVel &r2, double eps); + IMETHOD friend bool Equal(const Vector &r1, const VectorVel &r2, double eps); + IMETHOD friend bool Equal(const VectorVel &r1, const Vector &r2, double eps); + 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){} + 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; + } - 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); + IMETHOD friend bool Equal(const Rotation &r1, const RotationVel &r2, double eps); + IMETHOD friend bool Equal(const RotationVel &r1, const Rotation &r2, double eps); - - 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); - IMETHOD friend bool Equal(const Rotation& r1,const RotationVel& r2,double eps); - IMETHOD friend bool Equal(const RotationVel& r1,const Rotation& r2,double eps); - - 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; + 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 @@ -186,208 +190,213 @@ class FrameVel // = CAVEATS // { -public: - RotationVel M; - VectorVel p; -public: - FrameVel(){} + public: + RotationVel M; + VectorVel p; - explicit FrameVel(const Frame& T_): - M(T_.M),p(T_.p) {} + public: + FrameVel() {} - FrameVel(const Frame& T_,const Twist& _t): - M(T_.M,_t.rot),p(T_.p,_t.vel) {} + explicit FrameVel(const Frame &T_) : M(T_.M), p(T_.p) {} - FrameVel(const RotationVel& _M,const VectorVel& _p): - M(_M),p(_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());} + 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); + IMETHOD friend bool Equal(const Frame &r1, const FrameVel &r2, double eps); + IMETHOD friend bool Equal(const FrameVel &r1, const Frame &r2, double eps); - 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); - IMETHOD friend bool Equal(const Frame& r1,const FrameVel& r2,double eps); - IMETHOD friend bool Equal(const FrameVel& r1,const Frame& r2,double eps); - - 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; + 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. +// 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: + public: + VectorVel vel; + VectorVel rot; -// = 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) {}; + 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); + 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); + // = 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 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); + 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(); -// = Zero - static IMETHOD TwistVel Zero(); + // = Reverse Sign + IMETHOD void ReverseSign(); -// = 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 -// = 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); - IMETHOD friend bool Equal(const Twist& a,const TwistVel& b,double eps); - IMETHOD friend bool Equal(const TwistVel& a,const Twist& b,double eps); - -// = Conversion to other entities - IMETHOD Twist GetTwist() const; - IMETHOD Twist GetTwistDot() const; -// = Friends - friend class RotationVel; - friend class FrameVel; + // = 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); + IMETHOD friend bool Equal(const Twist &a, const TwistVel &b, double eps); + IMETHOD friend bool Equal(const TwistVel &a, const Twist &b, double eps); + // = Conversion to other entities + IMETHOD Twist GetTwist() const; + IMETHOD Twist GetTwistDot() const; + // = Friends + friend class RotationVel; + friend class FrameVel; }; -IMETHOD bool Equal(const VectorVel&, const VectorVel&, double = epsilon); -IMETHOD bool Equal(const Vector&, const VectorVel&, double = epsilon); -IMETHOD bool Equal(const VectorVel&, const Vector&, double = epsilon); -IMETHOD bool Equal(const RotationVel&, const RotationVel&, double = epsilon); -IMETHOD bool Equal(const Rotation&, const RotationVel&, double = epsilon); -IMETHOD bool Equal(const RotationVel&, const Rotation&, double = epsilon); -IMETHOD bool Equal(const FrameVel&, const FrameVel&, double = epsilon); -IMETHOD bool Equal(const Frame&, const FrameVel&, double = epsilon); -IMETHOD bool Equal(const FrameVel&, const Frame&, double = epsilon); -IMETHOD bool Equal(const TwistVel&, const TwistVel&, double = epsilon); -IMETHOD bool Equal(const Twist&, const TwistVel&, double = epsilon); -IMETHOD bool Equal(const TwistVel&, const Twist&, double = epsilon); +IMETHOD bool Equal(const VectorVel &, const VectorVel &, double = epsilon); +IMETHOD bool Equal(const Vector &, const VectorVel &, double = epsilon); +IMETHOD bool Equal(const VectorVel &, const Vector &, double = epsilon); +IMETHOD bool Equal(const RotationVel &, const RotationVel &, double = epsilon); +IMETHOD bool Equal(const Rotation &, const RotationVel &, double = epsilon); +IMETHOD bool Equal(const RotationVel &, const Rotation &, double = epsilon); +IMETHOD bool Equal(const FrameVel &, const FrameVel &, double = epsilon); +IMETHOD bool Equal(const Frame &, const FrameVel &, double = epsilon); +IMETHOD bool Equal(const FrameVel &, const Frame &, double = epsilon); +IMETHOD bool Equal(const TwistVel &, const TwistVel &, double = epsilon); +IMETHOD bool Equal(const Twist &, const TwistVel &, double = epsilon); +IMETHOD bool Equal(const TwistVel &, const Twist &, double = epsilon); -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 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 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 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 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 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 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(VectorVel &a) +{ + random(a.p); + random(a.v); } -IMETHOD void random(TwistVel& a) { - random(a.vel); - random(a.rot); +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(RotationVel &R) +{ + random(R.R); + random(R.w); } -IMETHOD void random(FrameVel& F) { - random(F.M); - random(F.p); +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(VectorVel &a) +{ + posrandom(a.p); + posrandom(a.v); } -IMETHOD void posrandom(TwistVel& a) { - posrandom(a.vel); - posrandom(a.rot); +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(RotationVel &R) +{ + posrandom(R.R); + posrandom(R.w); } -IMETHOD void posrandom(FrameVel& F) { - posrandom(F.M); - posrandom(F.p); +IMETHOD void posrandom(FrameVel &F) +{ + posrandom(F.M); + posrandom(F.p); } #ifdef KDL_INLINE -#include "framevel.inl" +# include "framevel.inl" #endif -} // namespace +} // namespace KDL #endif - - - - diff --git a/intern/itasc/kdl/inertia.cpp b/intern/itasc/kdl/inertia.cpp index 651666cf7b9..5e9c9fefd46 100644 --- a/intern/itasc/kdl/inertia.cpp +++ b/intern/itasc/kdl/inertia.cpp @@ -29,23 +29,19 @@ namespace KDL { using namespace Eigen; -Inertia::Inertia(double m,double Ixx,double Iyy,double Izz,double Ixy,double Ixz,double Iyz): -data(Matrix::Zero()) +Inertia::Inertia(double m, double Ixx, double Iyy, double Izz, double Ixy, double Ixz, double Iyz) + : data(Matrix::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(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::Identity(); + data.block(3, 3, 3, 3) = m * Matrix::Identity(); } -Inertia::~Inertia() -{ -} +Inertia::~Inertia() {} - - -} +} // namespace KDL diff --git a/intern/itasc/kdl/inertia.hpp b/intern/itasc/kdl/inertia.hpp index f37d9972883..fdfe0aae4ec 100644 --- a/intern/itasc/kdl/inertia.hpp +++ b/intern/itasc/kdl/inertia.hpp @@ -22,8 +22,8 @@ #ifndef KDLINERTIA_HPP #define KDLINERTIA_HPP -#include #include "frames.hpp" +#include namespace KDL { @@ -34,37 +34,39 @@ using namespace Eigen; * 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: +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); - /** - * 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); + }; - static inline Inertia Zero(){ - return Inertia(0,0,0,0,0,0,0); - }; + friend class Rotation; + friend class Frame; - friend class Rotation; - friend class Frame; + /** + * F = m*a + */ + // Wrench operator* (const AccelerationTwist& acc); - /** - * F = m*a - */ - // Wrench operator* (const AccelerationTwist& acc); - - - ~Inertia(); -private: - Matrix data; + ~Inertia(); + private: + Matrix data; }; - - - -} +} // namespace KDL #endif diff --git a/intern/itasc/kdl/jacobian.cpp b/intern/itasc/kdl/jacobian.cpp index e15eeac286c..88993c32128 100644 --- a/intern/itasc/kdl/jacobian.cpp +++ b/intern/itasc/kdl/jacobian.cpp @@ -24,109 +24,106 @@ #include "jacobian.hpp" -namespace KDL +namespace KDL { +Jacobian::Jacobian(unsigned int _size, unsigned int _nr_blocks) + : size(_size), nr_blocks(_nr_blocks) { - 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) - return NULL; - 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= size) + return NULL; + 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);}; + +} // namespace KDL diff --git a/intern/itasc/kdl/jntarray.hpp b/intern/itasc/kdl/jntarray.hpp index 886171b11db..b8d8aaa1376 100644 --- a/intern/itasc/kdl/jntarray.hpp +++ b/intern/itasc/kdl/jntarray.hpp @@ -25,199 +25,198 @@ #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). - +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 +JntArray j; +MyTask() +{} // invokes j's default constructor - bool configureHook() - { - unsigned int size = some_property.rvalue(); - j.resize(size) - ... - } +bool configureHook() +{ + unsigned int size = some_property.rvalue(); + j.resize(size) + ... +} - void updateHook() - { - ** use j here - } +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 - * - * - * @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) const; - /** - * set_item operator - * - * @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); - /** - * access operator for the joint array. Use pointer here to allow - * access to sequential joint angles (required for ndof joints) - * - * - * @return the joint value at position i, NULL if i is outside the valid range - */ - double* operator()(unsigned int i); - /** - * 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; +class JntArray { + private: + unsigned int size; + double *data; - /** - * 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); + 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); - friend bool operator==(const JntArray& src1,const JntArray& src2); - //friend bool operator!=(const JntArray& src1,const JntArray& src2); - }; - bool Equal(const JntArray&,const JntArray&, double = epsilon); - bool operator==(const JntArray& src1,const JntArray& src2); - //bool operator!=(const JntArray& src1,const JntArray& src2); + JntArray &operator=(const JntArray &arg); + /** + * get_item operator for the joint array + * + * + * @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) const; + /** + * set_item operator + * + * @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); + /** + * access operator for the joint array. Use pointer here to allow + * access to sequential joint angles (required for ndof joints) + * + * + * @return the joint value at position i, NULL if i is outside the valid range + */ + double *operator()(unsigned int i); + /** + * 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); + + friend bool operator==(const JntArray &src1, const JntArray &src2); + // friend bool operator!=(const JntArray& src1,const JntArray& src2); +}; +bool Equal(const JntArray &, const JntArray &, double = epsilon); +bool operator==(const JntArray &src1, const JntArray &src2); +// bool operator!=(const JntArray& src1,const JntArray& src2); + +} // namespace KDL #endif diff --git a/intern/itasc/kdl/jntarrayacc.cpp b/intern/itasc/kdl/jntarrayacc.cpp index 51e14e56f6f..0b4487c6123 100644 --- a/intern/itasc/kdl/jntarrayacc.cpp +++ b/intern/itasc/kdl/jntarrayacc.cpp @@ -24,150 +24,145 @@ #include "jntarrayacc.hpp" -namespace KDL +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) { - 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()) - { - } + 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)); - } +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)); +} +} // namespace KDL diff --git a/intern/itasc/kdl/jntarrayacc.hpp b/intern/itasc/kdl/jntarrayacc.hpp index fd1c26430e8..c899c61d6d1 100644 --- a/intern/itasc/kdl/jntarrayacc.hpp +++ b/intern/itasc/kdl/jntarrayacc.hpp @@ -22,46 +22,45 @@ #ifndef KDL_JNTARRAYACC_HPP #define KDL_JNTARRAYACC_HPP -#include "utilities/utility.h" +#include "frameacc.hpp" #include "jntarray.hpp" #include "jntarrayvel.hpp" -#include "frameacc.hpp" +#include "utilities/utility.h" -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); +namespace KDL { +class JntArrayAcc { + public: + JntArray q; + JntArray qdot; + JntArray qdotdot; - JntArray value()const; - JntArray deriv()const; - JntArray dderiv()const; + 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); - 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); - }; + JntArray value() const; + JntArray deriv() const; + JntArray dderiv() const; - bool Equal(const JntArrayAcc&, const JntArrayAcc&, double = epsilon); -} + 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); +}; + +bool Equal(const JntArrayAcc &, const JntArrayAcc &, double = epsilon); +} // namespace KDL #endif diff --git a/intern/itasc/kdl/jntarrayvel.cpp b/intern/itasc/kdl/jntarrayvel.cpp index 9f9328e88c4..c3cb9cae18b 100644 --- a/intern/itasc/kdl/jntarrayvel.cpp +++ b/intern/itasc/kdl/jntarrayvel.cpp @@ -22,93 +22,82 @@ // 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 +namespace KDL { +JntArrayVel::JntArrayVel(unsigned int size) : q(size), qdot(size) {} +JntArrayVel::JntArrayVel(const JntArray &qin, const JntArray &qdotin) : q(qin), qdot(qdotin) { - 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()) - { - } + 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); - } +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); +} +} // namespace KDL diff --git a/intern/itasc/kdl/jntarrayvel.hpp b/intern/itasc/kdl/jntarrayvel.hpp index 480f84f1708..58b94e051e6 100644 --- a/intern/itasc/kdl/jntarrayvel.hpp +++ b/intern/itasc/kdl/jntarrayvel.hpp @@ -22,39 +22,38 @@ #ifndef KDL_JNTARRAYVEL_HPP #define KDL_JNTARRAYVEL_HPP -#include "utilities/utility.h" -#include "jntarray.hpp" #include "framevel.hpp" +#include "jntarray.hpp" +#include "utilities/utility.h" -namespace KDL -{ +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); +class JntArrayVel { + public: + JntArray q; + JntArray qdot; - JntArray value()const; - JntArray deriv()const; + public: + JntArrayVel(unsigned int size); + JntArrayVel(const JntArray &q, const JntArray &qdot); + JntArrayVel(const JntArray &q); - 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); - }; + JntArray value() const; + JntArray deriv() const; - bool Equal(const JntArrayVel&, const JntArrayVel&, double = epsilon); -} + 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); +}; + +bool Equal(const JntArrayVel &, const JntArrayVel &, double = epsilon); +} // namespace KDL #endif diff --git a/intern/itasc/kdl/joint.cpp b/intern/itasc/kdl/joint.cpp index 2ec71066c66..528e6d0daff 100644 --- a/intern/itasc/kdl/joint.cpp +++ b/intern/itasc/kdl/joint.cpp @@ -26,139 +26,149 @@ 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 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(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::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() {} - Joint::~Joint() - { - } +Frame Joint::pose(const double *q) const +{ - Frame Joint::pose(const double* q)const - { + switch (type) { + case RotX: + assert(q); + return Frame(Rotation::RotX(scale * q[0] + offset)); + break; + case RotY: + assert(q); + return Frame(Rotation::RotY(scale * q[0] + offset)); + break; + case RotZ: + assert(q); + return Frame(Rotation::RotZ(scale * q[0] + offset)); + break; + case TransX: + assert(q); + return Frame(Vector(scale * q[0] + offset, 0.0, 0.0)); + break; + case TransY: + assert(q); + return Frame(Vector(0.0, scale * q[0] + offset, 0.0)); + break; + case TransZ: + assert(q); + return Frame(Vector(0.0, 0.0, scale * q[0] + 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) + assert(q); + 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) + assert(q); + return Frame(Rot(Vector(q[0], 0.0, q[1]))); + break; + default: + return Frame::Identity(); + break; + } +} - switch(type){ - case RotX: - assert(q); - return Frame(Rotation::RotX(scale*q[0]+offset)); - break; - case RotY: - assert(q); - return Frame(Rotation::RotY(scale*q[0]+offset)); - break; - case RotZ: - assert(q); - return Frame(Rotation::RotZ(scale*q[0]+offset)); - break; - case TransX: - assert(q); - return Frame(Vector(scale*q[0]+offset,0.0,0.0)); - break; - case TransY: - assert(q); - return Frame(Vector(0.0,scale*q[0]+offset,0.0)); - break; - case TransZ: - assert(q); - return Frame(Vector(0.0,0.0,scale*q[0]+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) - assert(q); - 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) - assert(q); - 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; + } +} - 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 +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 index 9d25b427499..388097f4983 100644 --- a/intern/itasc/kdl/joint.hpp +++ b/intern/itasc/kdl/joint.hpp @@ -27,112 +27,116 @@ 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); +/** + * \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); + 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 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 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; + /** + * 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(); + virtual ~Joint(); - private: - Joint::JointType type; - double scale; - double offset; - double inertia; - double damping; - double stiffness; - }; + private: + Joint::JointType type; + double scale; + double offset; + double inertia; + double damping; + double stiffness; +}; -} // end of namespace KDL +} // end of namespace KDL #endif diff --git a/intern/itasc/kdl/kinfam_io.cpp b/intern/itasc/kdl/kinfam_io.cpp index 5ae62648c2b..c89a718622c 100644 --- a/intern/itasc/kdl/kinfam_io.cpp +++ b/intern/itasc/kdl/kinfam_io.cpp @@ -26,79 +26,92 @@ #include "frames_io.hpp" namespace KDL { -std::ostream& operator <<(std::ostream& os, const Joint& joint) { - return os << joint.getTypeName(); +std::ostream &operator<<(std::ostream &os, const Joint &joint) +{ + return os << joint.getTypeName(); } -std::istream& operator >>(std::istream& is, Joint& joint) { - return is; +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::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::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::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::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, const Tree &tree) +{ + SegmentMap::const_iterator root = tree.getSegment("root"); + return os << root; } -std::ostream& operator <<(std::ostream& os, SegmentMap::const_iterator root) { - //os<first<<": "<second.segment<<"\n"; - os << root->first<<"(q_nr: "<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::ostream &operator<<(std::ostream &os, SegmentMap::const_iterator root) +{ + // os<first<<": "<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::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::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::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; +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; } +} // namespace KDL diff --git a/intern/itasc/kdl/kinfam_io.hpp b/intern/itasc/kdl/kinfam_io.hpp index a8dbfd1c5dc..3564c1afc20 100644 --- a/intern/itasc/kdl/kinfam_io.hpp +++ b/intern/itasc/kdl/kinfam_io.hpp @@ -22,49 +22,46 @@ #ifndef KDL_KINFAM_IO_HPP #define KDL_KINFAM_IO_HPP -#include #include +#include +#include "chain.hpp" +#include "jacobian.hpp" +#include "jntarray.hpp" #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 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, 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, 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); +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 -std::ostream& operator<<(std::ostream& os, const std::vector& vec) { - os << "["; - for (unsigned int i = 0; i < vec.size(); i++) - os << vec[i] << " "; - os << "]"; - return os; -} -; +template std::ostream &operator<<(std::ostream &os, const std::vector &vec) +{ + os << "["; + for (unsigned int i = 0; i < vec.size(); i++) + os << vec[i] << " "; + os << "]"; + return os; +}; -template -std::istream& operator >>(std::istream& is, std::vector& vec) { - return is; -} -; -} +template std::istream &operator>>(std::istream &is, std::vector &vec) +{ + return is; +}; +} // namespace KDL #endif - diff --git a/intern/itasc/kdl/segment.cpp b/intern/itasc/kdl/segment.cpp index b116095c8b2..191f0248dea 100644 --- a/intern/itasc/kdl/segment.cpp +++ b/intern/itasc/kdl/segment.cpp @@ -24,48 +24,40 @@ namespace KDL { - Segment::Segment(const Joint& _joint, const Frame& _f_tip, const Inertia& _M): - M(_M),joint(_joint), - f_tip(_f_tip) - { - } +Segment::Segment(const Joint &_joint, const Frame &_f_tip, const Inertia &_M) + : M(_M), joint(_joint), f_tip(_f_tip) +{ +} - Segment::Segment(const Segment& in): - M(in.M),joint(in.joint), - f_tip(in.f_tip) - { - } +Segment::Segment(const Segment &in) : M(in.M), joint(in.joint), 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::operator=(const Segment &arg) +{ + joint = arg.joint; + M = arg.M; + f_tip = arg.f_tip; + return *this; +} - Segment::~Segment() - { - } +Segment::~Segment() {} - Frame Segment::pose(const double* q)const - { - return joint.pose(q)*f_tip; - } +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 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 +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 index 130bfb13f8b..3acea39ec6e 100644 --- a/intern/itasc/kdl/segment.hpp +++ b/intern/itasc/kdl/segment.hpp @@ -19,7 +19,6 @@ // 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 @@ -30,120 +29,122 @@ 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: - Inertia M; - Joint joint; - Frame f_tip; +/** + * \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; - 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); + private: + Inertia M; + Joint joint; + Frame f_tip; - virtual ~Segment(); + 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); - /** - * 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; + virtual ~Segment(); - /** - * 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 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 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 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 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 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 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; - } + /** + * 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; + } - }; -}//end of namespace KDL + /** + * 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 index 39c7f58d433..1ba0b3fadad 100644 --- a/intern/itasc/kdl/tree.cpp +++ b/intern/itasc/kdl/tree.cpp @@ -27,100 +27,104 @@ namespace KDL { using namespace std; -Tree::Tree() : - nrOfJoints(0), nrOfSegments(0) { - TreeElement root; - std::pair val("root", root); - segments.insert(val); +Tree::Tree() : nrOfJoints(0), nrOfSegments(0) +{ + TreeElement root; + std::pair val("root", root); + segments.insert(val); } -Tree::Tree(const Tree& in) { - segments.clear(); - nrOfSegments = 0; - nrOfJoints = 0; - TreeElement root; - std::pair val("root", root); - segments.insert(val); - this->addTree(in, "", "root"); - +Tree::Tree(const Tree &in) +{ + segments.clear(); + nrOfSegments = 0; + nrOfJoints = 0; + TreeElement root; + std::pair val("root", root); + segments.insert(val); + this->addTree(in, "", "root"); } -Tree& Tree::operator=(const Tree& in) { - segments.clear(); - nrOfSegments = 0; - nrOfJoints = 0; - TreeElement root; - std::pair val("root", root); - segments.insert(val); - this->addTree(in, "", "root"); - return *this; +Tree &Tree::operator=(const Tree &in) +{ + segments.clear(); + nrOfSegments = 0; + nrOfJoints = 0; + TreeElement root; + std::pair val("root", root); + segments.insert(val); + 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 retval; - //insert new element - TreeElement elem(segment, *parent, nrOfJoints); - std::pair val(segment_name, elem); +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 retval; + // insert new element + TreeElement elem(segment, *parent, nrOfJoints); + std::pair val(segment_name, elem); - retval = segments.insert(val); - //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; + retval = segments.insert(val); + // 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::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::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; + 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; } - return true; -} - + else + // If the child could not be added, return false + return false; + } + return true; } +} // namespace KDL diff --git a/intern/itasc/kdl/tree.hpp b/intern/itasc/kdl/tree.hpp index bd35f82d185..8e73a74058a 100644 --- a/intern/itasc/kdl/tree.hpp +++ b/intern/itasc/kdl/tree.hpp @@ -22,156 +22,161 @@ #ifndef KDL_TREE_HPP #define KDL_TREE_HPP -#include "segment.hpp" #include "chain.hpp" +#include "segment.hpp" -#include -#include #include +#include +#include -namespace KDL -{ - //Forward declaration - class TreeElement; - // Eigen allocator is needed for alignment of Eigen data types - typedef std::map, Eigen::aligned_allocator > > SegmentMap; +namespace KDL { +// Forward declaration +class TreeElement; +// Eigen allocator is needed for alignment of Eigen data types +typedef std::map, + Eigen::aligned_allocator>> + SegmentMap; - class TreeElement - { - public: - TreeElement():q_nr(0),parent(0) - {}; - public: - Segment segment; - unsigned int q_nr; - SegmentMap::value_type const *parent; - std::vector children; - TreeElement(const Segment& segment_in,const SegmentMap::value_type& parent_in,unsigned int q_nr_in) - { - q_nr=q_nr_in; - segment=segment_in; - parent=&parent_in; - }; - static TreeElement Root() - { - return TreeElement(); - }; - }; +class TreeElement { + public: + TreeElement() : q_nr(0), parent(0){}; - /** - * \brief This class encapsulates a tree - * kinematic interconnection structure. It is build out of segments. - * - * @ingroup KinematicFamily - */ - class Tree - { - private: - SegmentMap segments; - unsigned int nrOfJoints; - unsigned int nrOfSegments; + public: + Segment segment; + unsigned int q_nr; + SegmentMap::value_type const *parent; + std::vector children; + TreeElement(const Segment &segment_in, + const SegmentMap::value_type &parent_in, + unsigned int q_nr_in) + { + q_nr = q_nr_in; + segment = segment_in; + parent = &parent_in; + }; + static TreeElement Root() + { + return TreeElement(); + }; +}; - bool addTreeRecursive(SegmentMap::const_iterator root, const std::string& tree_name, const std::string& hook_name); +/** + * \brief This class encapsulates a tree + * kinematic interconnection structure. It is build out of segments. + * + * @ingroup KinematicFamily + */ +class Tree { + private: + SegmentMap segments; + unsigned int nrOfJoints; + unsigned int nrOfSegments; - public: - /** - * The constructor of a tree, a new tree is always empty - */ - Tree(); - Tree(const Tree& in); - Tree& operator= (const Tree& arg); + bool addTreeRecursive(SegmentMap::const_iterator root, + const std::string &tree_name, + const std::string &hook_name); - /** - * 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); + public: + /** + * The constructor of a tree, a new tree is always empty + */ + Tree(); + Tree(const Tree &in); + Tree &operator=(const Tree &arg); - /** - * 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 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 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); + /** + * 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); - /** - * Request the total number of joints in the tree.\n - * Important: 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; - }; + /** + * 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 segments in the tree. - * @return total number of segments - */ - unsigned int getNrOfSegments()const {return nrOfSegments;}; + /** + * Request the total number of joints in the tree.\n + * Important: 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 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); - }; + /** + * Request the total number of segments in the tree. + * @return total number of segments + */ + unsigned int getNrOfSegments() const + { + return nrOfSegments; + }; - SegmentMap::value_type const* getSegmentPtr(const std::string& segment_name)const - { - SegmentMap::const_iterator it = segments.find(segment_name); - - if (it == segments.end()) - return 0; + /** + * 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); + }; - return &*it; - }; + SegmentMap::value_type const *getSegmentPtr(const std::string &segment_name) const + { + SegmentMap::const_iterator it = segments.find(segment_name); - const SegmentMap& getSegments()const - { - return segments; - } + if (it == segments.end()) + return 0; - virtual ~Tree(){}; - }; -} + return &*it; + }; + + const SegmentMap &getSegments() const + { + return segments; + } + + virtual ~Tree(){}; +}; +} // namespace KDL #endif - - - - - diff --git a/intern/itasc/kdl/treefksolver.hpp b/intern/itasc/kdl/treefksolver.hpp index 22d5400ab0a..e7c91aaa071 100644 --- a/intern/itasc/kdl/treefksolver.hpp +++ b/intern/itasc/kdl/treefksolver.hpp @@ -26,85 +26,87 @@ #include #include "tree.hpp" -//#include "framevel.hpp" -//#include "frameacc.hpp" +// #include "framevel.hpp" +// #include "frameacc.hpp" #include "jntarray.hpp" -//#include "jntarrayvel.hpp" -//#include "jntarrayacc.hpp" +// #include "jntarrayvel.hpp" +// #include "jntarrayacc.hpp" namespace KDL { - /** - * \brief This abstract class encapsulates a - * solver for the forward position kinematics for a KDL::Tree. - * - * @ingroup KinematicFamily - */ +/** + * \brief This abstract 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(){}; - }; +// 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 abstract class encapsulates a solver - * for the forward velocity kinematics for a KDL::Tree. - * - * @ingroup KinematicFamily - */ +/** + * \brief This abstract 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 - */ +/** + * 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 abstract class encapsulates a solver - * for the forward acceleration kinematics for a KDL::Tree. - * - * @ingroup KinematicFamily - */ + +/** + * \brief This abstract 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 - */ +/** + * 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 +} // end of namespace KDL #endif diff --git a/intern/itasc/kdl/treefksolverpos_recursive.cpp b/intern/itasc/kdl/treefksolverpos_recursive.cpp index f465e13a84c..4c31ed157fd 100644 --- a/intern/itasc/kdl/treefksolverpos_recursive.cpp +++ b/intern/itasc/kdl/treefksolverpos_recursive.cpp @@ -28,45 +28,44 @@ 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::value_type const* it = tree.getSegmentPtr(segmentName); - SegmentMap::value_type const* baseit = tree.getSegmentPtr(baseName); - - if(q_in.rows() != tree.getNrOfJoints()) - return -1; - else if(!it) //if the segment name is not found - return -2; - else if(!baseit) //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, SegmentMap::value_type const* it, SegmentMap::value_type const* 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)); - return recursiveFk(q_in, currentElement.parent, baseit) * currentFrame; - } - } - - TreeFkSolverPos_recursive::~TreeFkSolverPos_recursive() - { - } +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::value_type const *it = tree.getSegmentPtr(segmentName); + SegmentMap::value_type const *baseit = tree.getSegmentPtr(baseName); + if (q_in.rows() != tree.getNrOfJoints()) + return -1; + else if (!it) // if the segment name is not found + return -2; + else if (!baseit) // 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, + SegmentMap::value_type const *it, + SegmentMap::value_type const *baseit) +{ + // gets the frame for the current element (segment) + const TreeElement ¤tElement = it->second; + + if (it == baseit) { + return KDL::Frame::Identity(); + } + else { + Frame currentFrame = currentElement.segment.pose(((JntArray &)q_in)(currentElement.q_nr)); + return recursiveFk(q_in, currentElement.parent, baseit) * currentFrame; + } +} + +TreeFkSolverPos_recursive::~TreeFkSolverPos_recursive() {} + +} // namespace KDL diff --git a/intern/itasc/kdl/treefksolverpos_recursive.hpp b/intern/itasc/kdl/treefksolverpos_recursive.hpp index 2081f23a9ff..71e17bc2984 100644 --- a/intern/itasc/kdl/treefksolverpos_recursive.hpp +++ b/intern/itasc/kdl/treefksolverpos_recursive.hpp @@ -27,27 +27,31 @@ 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(); +/** + * 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); + 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, SegmentMap::value_type const* it, SegmentMap::value_type const* baseit); - }; + private: + const Tree tree; -} + Frame recursiveFk(const JntArray &q_in, + SegmentMap::value_type const *it, + SegmentMap::value_type const *baseit); +}; + +} // namespace KDL #endif diff --git a/intern/itasc/kdl/treejnttojacsolver.cpp b/intern/itasc/kdl/treejnttojacsolver.cpp index 94ecad7aec5..2f0ad21266e 100644 --- a/intern/itasc/kdl/treejnttojacsolver.cpp +++ b/intern/itasc/kdl/treejnttojacsolver.cpp @@ -13,69 +13,66 @@ namespace KDL { -TreeJntToJacSolver::TreeJntToJacSolver(const Tree& tree_in) : - tree(tree_in) { -} +TreeJntToJacSolver::TreeJntToJacSolver(const Tree &tree_in) : tree(tree_in) {} -TreeJntToJacSolver::~TreeJntToJacSolver() { -} +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; +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::value_type const* it = tree.getSegmentPtr(segmentname); + // Lets search the tree-element + SegmentMap::value_type const *it = tree.getSegmentPtr(segmentname); - //If segmentname is not inside the tree, back out: - if (!it) - return -2; + // If segmentname is not inside the tree, back out: + if (!it) + return -2; - //Let's make the jacobian zero: - SetToZero(jac); + // Let's make the jacobian zero: + SetToZero(jac); - SegmentMap::value_type const* root = tree.getSegmentPtr("root"); + SegmentMap::value_type const *root = tree.getSegmentPtr("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; + 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 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; dofsecond.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); + // 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 + return 0; +} // end JntToJac +} // namespace KDL diff --git a/intern/itasc/kdl/treejnttojacsolver.hpp b/intern/itasc/kdl/treejnttojacsolver.hpp index 40977dcd577..d6dc54d829d 100644 --- a/intern/itasc/kdl/treejnttojacsolver.hpp +++ b/intern/itasc/kdl/treejnttojacsolver.hpp @@ -8,31 +8,30 @@ #ifndef TREEJNTTOJACSOLVER_HPP_ #define TREEJNTTOJACSOLVER_HPP_ -#include "tree.hpp" #include "jacobian.hpp" #include "jntarray.hpp" +#include "tree.hpp" namespace KDL { class TreeJntToJacSolver { -public: - TreeJntToJacSolver(const Tree& tree); + public: + TreeJntToJacSolver(const Tree &tree); - virtual ~TreeJntToJacSolver(); + 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 - */ + /* + * 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; + int JntToJac(const JntArray &q_in, Jacobian &jac, const std::string &segmentname); + private: + KDL::Tree tree; }; -}//End of namespace +} // namespace KDL #endif /* TREEJNTTOJACSOLVER_H_ */ diff --git a/intern/itasc/kdl/utilities/error.h b/intern/itasc/kdl/utilities/error.h index c6ab3ebdc69..219c66e740e 100644 --- a/intern/itasc/kdl/utilities/error.h +++ b/intern/itasc/kdl/utilities/error.h @@ -1,12 +1,12 @@ /*************************************************************************** - tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 error.h + 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 * @@ -24,23 +24,22 @@ * Fifth Floor, Boston, MA 02110-1301, USA. * * * ***************************************************************************/ - - + /***************************************************************************** - * \file + * \file * Defines the exception classes that can be thrown * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * - * \version + * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release - * $Name: $ + * $Name: $ ****************************************************************************/ -#ifndef ERROR_H_84822 // to make it unique, a random number +#ifndef ERROR_H_84822 // to make it unique, a random number #define ERROR_H_84822 #include "utility.h" @@ -48,196 +47,362 @@ namespace KDL { -/** - * Base class for errors generated by ORO_Geometry +/** + * Base class for errors generated by ORO_Geometry */ class Error { -public: - /** Returns a description string describing the error. - * the returned pointer only guaranteed to exists as long as - * the Error object exists. - */ - virtual ~Error() {} - virtual const char* Description() const {return "Unspecified Error\n";} + public: + /** Returns a description string describing the error. + * the returned pointer only guaranteed 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;} + 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;} + 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;} + 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;} + 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;} + 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;} + 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;} + 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;} + 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;} + 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;} + 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;} + 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;} + 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_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;} + 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 : 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;} + 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_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;} + 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;} + 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 too small";} - virtual int GetType() const {return 3001;} + public: + virtual const char *Description() const + { + return "Circle : radius is too 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;} + 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_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_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;} +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;} +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;} + 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 +//! 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;} + 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_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_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;} +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; + } }; - - -} +} // namespace KDL #endif diff --git a/intern/itasc/kdl/utilities/error_stack.cpp b/intern/itasc/kdl/utilities/error_stack.cpp index a409effb193..6ea38881a5b 100644 --- a/intern/itasc/kdl/utilities/error_stack.cpp +++ b/intern/itasc/kdl/utilities/error_stack.cpp @@ -4,57 +4,57 @@ /***************************************************************************** * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * - * \version + * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release - * $Name: $ + * $Name: $ ****************************************************************************/ - #include "error_stack.h" -#include -#include -#include #include +#include +#include +#include namespace KDL { // Trace of the call stack of the I/O routines to help user // interprete error messages from I/O -typedef std::stack ErrorStack; +typedef std::stack 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 IOTrace(const std::string &description) +{ + errorstack.push(description); } +void IOTracePop() +{ + errorstack.pop(); +} -void IOTracePop() { +void IOTraceOutput(std::ostream &os) +{ + while (!errorstack.empty()) { + os << errorstack.top().c_str() << std::endl; 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(); } - -void IOTracePopStr(char* buffer,int size) { - if (errorstack.empty()) { - *buffer = 0; - return; - } - strncpy(buffer,errorstack.top().c_str(),size); - errorstack.pop(); -} - -} +} // namespace KDL diff --git a/intern/itasc/kdl/utilities/error_stack.h b/intern/itasc/kdl/utilities/error_stack.h index 95ccc2d83eb..d15d214759f 100644 --- a/intern/itasc/kdl/utilities/error_stack.h +++ b/intern/itasc/kdl/utilities/error_stack.h @@ -1,12 +1,12 @@ /*************************************************************************** - tag: Erwin Aertbelien Mon Jan 10 16:38:39 CET 2005 error_stack.h + 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 * @@ -24,13 +24,12 @@ * Fifth Floor, Boston, MA 02110-1301, USA. * * * ***************************************************************************/ - - -/** - * \file - * \version + +/** + * \file + * \version * ORO_Geometry V0.2 - * + * * \par history * - changed layout of the comments to accommodate doxygen */ @@ -41,7 +40,6 @@ #include "utility_io.h" #include - namespace KDL { /* @@ -49,21 +47,18 @@ namespace KDL { * 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); +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); +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); +void IOTracePopStr(char *buffer, int size); - -} +} // namespace KDL #endif - diff --git a/intern/itasc/kdl/utilities/kdl-config.h b/intern/itasc/kdl/utilities/kdl-config.h index b49ddf91058..a0a98dcbf6e 100644 --- a/intern/itasc/kdl/utilities/kdl-config.h +++ b/intern/itasc/kdl/utilities/kdl-config.h @@ -22,7 +22,6 @@ /* 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 diff --git a/intern/itasc/kdl/utilities/rall1d.h b/intern/itasc/kdl/utilities/rall1d.h index a8e317de127..5a703c50fad 100644 --- a/intern/itasc/kdl/utilities/rall1d.h +++ b/intern/itasc/kdl/utilities/rall1d.h @@ -1,476 +1,461 @@ - + /***************************************************************************** - * \file - * class for automatic differentiation on scalar values and 1st + * \file + * class for automatic differentiation on scalar values and 1st * derivatives . - * + * * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * - * \version + * \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 ! + * 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$ + * - $log$ * * \par Release - * $Name: $ + * $Name: $ ****************************************************************************/ - + #ifndef Rall1D_H #define Rall1D_H -#include #include "utility.h" +#include 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. + * - 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,.. + * S is usefull when you recurse a Rall1d object into itself to create a 2nd, 3th, 4th,.. * derivatives. (e.g. Rall1d< Rall1d, Rall1d, double> ). * - * S is always passed by value. + * S is always passed by value. * * \par Class Type * Concrete implementation */ -template -class Rall1d - { - public: - typedef T valuetype; - typedef V gradienttype; - typedef S scalartype; - public : - T t; //!< value - V grad; //!< gradient - public : - INLINE Rall1d() {} +template class Rall1d { + public: + typedef T valuetype; + typedef V gradienttype; + typedef S scalartype; - T value() const { - return t; - } - V deriv() const { - return grad; - } + public: + T t; //!< value + V grad; //!< gradient + public: + INLINE Rall1d() {} - explicit INLINE Rall1d(typename TI::Arg c) - {t=T(c);SetToZero(grad);} + T value() const + { + return t; + } + V deriv() const + { + return grad; + } - INLINE Rall1d(typename TI::Arg tn, typename TI::Arg afg):t(tn),grad(afg) {} + explicit INLINE Rall1d(typename TI::Arg c) + { + t = T(c); + SetToZero(grad); + } - INLINE Rall1d(const Rall1d& 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 Rall1d(typename TI::Arg tn, typename TI::Arg afg) : t(tn), grad(afg) {} - INLINE V& Gradient() { - return grad; - } + INLINE Rall1d(const Rall1d &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 static Rall1d Zero() { - Rall1d tmp; - SetToZero(tmp); - return tmp; - } - INLINE static Rall1d Identity() { - Rall1d tmp; - SetToIdentity(tmp); - return tmp; - } + INLINE T &Value() + { + return t; + } - INLINE Rall1d& operator =(S c) - {t=c;SetToZero(grad);return *this;} + INLINE V &Gradient() + { + return grad; + } - INLINE Rall1d& operator =(const Rall1d& r) - {t=r.t;grad=r.grad;return *this;} + INLINE static Rall1d Zero() + { + Rall1d tmp; + SetToZero(tmp); + return tmp; + } + INLINE static Rall1d Identity() + { + Rall1d tmp; + SetToIdentity(tmp); + return tmp; + } - INLINE Rall1d& operator /=(const Rall1d& rhs) - { - grad = LinComb(rhs.t,grad,-t,rhs.grad) / (rhs.t*rhs.t); - t /= rhs.t; - return *this; - } + INLINE Rall1d &operator=(S c) + { + t = c; + SetToZero(grad); + return *this; + } - INLINE Rall1d& operator *=(const Rall1d& rhs) - { - LinCombR(rhs.t,grad,t,rhs.grad,grad); - t *= rhs.t; - return *this; - } + INLINE Rall1d &operator=(const Rall1d &r) + { + t = r.t; + grad = r.grad; + return *this; + } - INLINE Rall1d& operator +=(const Rall1d& rhs) - { - grad +=rhs.grad; - t +=rhs.t; - return *this; - } + INLINE Rall1d &operator/=(const Rall1d &rhs) + { + grad = LinComb(rhs.t, grad, -t, rhs.grad) / (rhs.t * rhs.t); + t /= rhs.t; + return *this; + } - INLINE Rall1d& operator -=(const Rall1d& rhs) - { - grad -= rhs.grad; - t -= rhs.t; - return *this; - } + INLINE Rall1d &operator*=(const Rall1d &rhs) + { + LinCombR(rhs.t, grad, t, rhs.grad, grad); + t *= rhs.t; + return *this; + } - INLINE Rall1d& operator /=(S rhs) - { - grad /= rhs; - t /= rhs; - return *this; - } + INLINE Rall1d &operator+=(const Rall1d &rhs) + { + grad += rhs.grad; + t += rhs.t; + return *this; + } - INLINE Rall1d& operator *=(S rhs) - { - grad *= rhs; - t *= rhs; - return *this; - } + INLINE Rall1d &operator-=(const Rall1d &rhs) + { + grad -= rhs.grad; + t -= rhs.t; + return *this; + } - INLINE Rall1d& operator +=(S rhs) - { - t += rhs; - return *this; - } + INLINE Rall1d &operator/=(S rhs) + { + grad /= rhs; + t /= rhs; + return *this; + } - INLINE Rall1d& operator -=(S rhs) - { - t -= rhs; - return *this; - } + INLINE Rall1d &operator*=(S rhs) + { + grad *= rhs; + t *= rhs; + return *this; + } + INLINE Rall1d &operator+=(S rhs) + { + t += rhs; + return *this; + } + INLINE Rall1d &operator-=(S rhs) + { + t -= rhs; + return *this; + } - // = operators - /* gives warnings on cygwin - - template - friend INLINE Rall1d operator /(const Rall1d& lhs,const Rall1d& rhs); - - friend INLINE Rall1d operator *(const Rall1d& lhs,const Rall1d& rhs); - friend INLINE Rall1d operator +(const Rall1d& lhs,const Rall1d& rhs); - friend INLINE Rall1d operator -(const Rall1d& lhs,const Rall1d& rhs); - friend INLINE Rall1d operator -(const Rall1d& arg); - friend INLINE Rall1d operator *(S s,const Rall1d& v); - friend INLINE Rall1d operator *(const Rall1d& v,S s); - friend INLINE Rall1d operator +(S s,const Rall1d& v); - friend INLINE Rall1d operator +(const Rall1d& v,S s); - friend INLINE Rall1d operator -(S s,const Rall1d& v); - friend INLINE Rall1d operator -(const Rall1d& v,S s); - friend INLINE Rall1d operator /(S s,const Rall1d& v); - friend INLINE Rall1d operator /(const Rall1d& v,S s); + // = operators + /* gives warnings on cygwin - // = Mathematical functions that operate on Rall1d objects - friend INLINE Rall1d exp(const Rall1d& arg); - friend INLINE Rall1d log(const Rall1d& arg); - friend INLINE Rall1d sin(const Rall1d& arg); - friend INLINE Rall1d cos(const Rall1d& arg); - friend INLINE Rall1d tan(const Rall1d& arg); - friend INLINE Rall1d sinh(const Rall1d& arg); - friend INLINE Rall1d cosh(const Rall1d& arg); - friend INLINE Rall1d sqr(const Rall1d& arg); - friend INLINE Rall1d pow(const Rall1d& arg,double m) ; - friend INLINE Rall1d sqrt(const Rall1d& arg); - friend INLINE Rall1d atan(const Rall1d& x); - friend INLINE Rall1d hypot(const Rall1d& y,const Rall1d& x); - friend INLINE Rall1d asin(const Rall1d& x); - friend INLINE Rall1d acos(const Rall1d& x); - friend INLINE Rall1d abs(const Rall1d& x); - friend INLINE S Norm(const Rall1d& value) ; - friend INLINE Rall1d tanh(const Rall1d& arg); - friend INLINE Rall1d atan2(const Rall1d& y,const Rall1d& x); - - // = Utility functions to improve performance + template + friend INLINE Rall1d operator /(const Rall1d& lhs,const Rall1d& + rhs); - friend INLINE Rall1d LinComb(S alfa,const Rall1d& a, - const T& beta,const Rall1d& b ); - - friend INLINE void LinCombR(S alfa,const Rall1d& a, - const T& beta,const Rall1d& b,Rall1d& result ); - - // = Setting value of a Rall1d object to 0 or 1 + friend INLINE Rall1d operator *(const Rall1d& lhs,const Rall1d& rhs); + friend INLINE Rall1d operator +(const Rall1d& lhs,const Rall1d& rhs); + friend INLINE Rall1d operator -(const Rall1d& lhs,const Rall1d& rhs); + friend INLINE Rall1d operator -(const Rall1d& arg); + friend INLINE Rall1d operator *(S s,const Rall1d& v); + friend INLINE Rall1d operator *(const Rall1d& v,S s); + friend INLINE Rall1d operator +(S s,const Rall1d& v); + friend INLINE Rall1d operator +(const Rall1d& v,S s); + friend INLINE Rall1d operator -(S s,const Rall1d& v); + friend INLINE Rall1d operator -(const Rall1d& v,S s); + friend INLINE Rall1d operator /(S s,const Rall1d& v); + friend INLINE Rall1d operator /(const Rall1d& v,S s); - friend INLINE void SetToZero(Rall1d& value); - friend INLINE void SetToOne(Rall1d& value); - // = Equality in an eps-interval - friend INLINE bool Equal(const Rall1d& y,const Rall1d& x,double eps); - */ - }; + // = Mathematical functions that operate on Rall1d objects + friend INLINE Rall1d exp(const Rall1d& arg); + friend INLINE Rall1d log(const Rall1d& arg); + friend INLINE Rall1d sin(const Rall1d& arg); + friend INLINE Rall1d cos(const Rall1d& arg); + friend INLINE Rall1d tan(const Rall1d& arg); + friend INLINE Rall1d sinh(const Rall1d& arg); + friend INLINE Rall1d cosh(const Rall1d& arg); + friend INLINE Rall1d sqr(const Rall1d& arg); + friend INLINE Rall1d pow(const Rall1d& arg,double m) ; + friend INLINE Rall1d sqrt(const Rall1d& arg); + friend INLINE Rall1d atan(const Rall1d& x); + friend INLINE Rall1d hypot(const Rall1d& y,const Rall1d& x); + friend INLINE Rall1d asin(const Rall1d& x); + friend INLINE Rall1d acos(const Rall1d& x); + friend INLINE Rall1d abs(const Rall1d& x); + friend INLINE S Norm(const Rall1d& value) ; + friend INLINE Rall1d tanh(const Rall1d& arg); + friend INLINE Rall1d atan2(const Rall1d& y,const Rall1d& x); + // = Utility functions to improve performance -template -INLINE Rall1d operator /(const Rall1d& lhs,const Rall1d& rhs) - { - return Rall1d(lhs.t/rhs.t,(lhs.grad*rhs.t-lhs.t*rhs.grad)/(rhs.t*rhs.t)); - } + friend INLINE Rall1d LinComb(S alfa,const Rall1d& a, + const T& beta,const Rall1d& b ); -template -INLINE Rall1d operator *(const Rall1d& lhs,const Rall1d& rhs) - { - return Rall1d(lhs.t*rhs.t,rhs.t*lhs.grad+lhs.t*rhs.grad); - } + friend INLINE void LinCombR(S alfa,const Rall1d& a, + const T& beta,const Rall1d& b,Rall1d& result ); -template -INLINE Rall1d operator +(const Rall1d& lhs,const Rall1d& rhs) - { - return Rall1d(lhs.t+rhs.t,lhs.grad+rhs.grad); - } + // = Setting value of a Rall1d object to 0 or 1 + friend INLINE void SetToZero(Rall1d& value); + friend INLINE void SetToOne(Rall1d& value); + // = Equality in an eps-interval + friend INLINE bool Equal(const Rall1d& y,const Rall1d& x,double eps); + */ +}; -template -INLINE Rall1d operator -(const Rall1d& lhs,const Rall1d& rhs) - { - return Rall1d(lhs.t-rhs.t,lhs.grad-rhs.grad); - } - -template -INLINE Rall1d operator -(const Rall1d& arg) - { - return Rall1d(-arg.t,-arg.grad); - } - -template -INLINE Rall1d operator *(S s,const Rall1d& v) - { - return Rall1d(s*v.t,s*v.grad); - } - -template -INLINE Rall1d operator *(const Rall1d& v,S s) - { - return Rall1d(v.t*s,v.grad*s); - } - -template -INLINE Rall1d operator +(S s,const Rall1d& v) - { - return Rall1d(s+v.t,v.grad); - } - -template -INLINE Rall1d operator +(const Rall1d& v,S s) - { - return Rall1d(v.t+s,v.grad); - } - -template -INLINE Rall1d operator -(S s,const Rall1d& v) - { - return Rall1d(s-v.t,-v.grad); - } - -template -INLINE Rall1d operator -(const Rall1d& v,S s) - { - return Rall1d(v.t-s,v.grad); - } - -template -INLINE Rall1d operator /(S s,const Rall1d& v) - { - return Rall1d(s/v.t,(-s*v.grad)/(v.t*v.t)); - } - -template -INLINE Rall1d operator /(const Rall1d& v,S s) - { - return Rall1d(v.t/s,v.grad/s); - } - - -template -INLINE Rall1d exp(const Rall1d& arg) - { - T v; - v= (exp(arg.t)); - return Rall1d(v,v*arg.grad); - } - -template -INLINE Rall1d log(const Rall1d& arg) - { - T v; - v=(log(arg.t)); - return Rall1d(v,arg.grad/arg.t); - } - -template -INLINE Rall1d sin(const Rall1d& arg) - { - T v; - v=(sin(arg.t)); - return Rall1d(v,cos(arg.t)*arg.grad); - } - -template -INLINE Rall1d cos(const Rall1d& arg) - { - T v; - v=(cos(arg.t)); - return Rall1d(v,-sin(arg.t)*arg.grad); - } - -template -INLINE Rall1d tan(const Rall1d& arg) - { - T v; - v=(tan(arg.t)); - return Rall1d(v,arg.grad/sqr(cos(arg.t))); - } - -template -INLINE Rall1d sinh(const Rall1d& arg) - { - T v; - v=(sinh(arg.t)); - return Rall1d(v,cosh(arg.t)*arg.grad); - } - -template -INLINE Rall1d cosh(const Rall1d& arg) - { - T v; - v=(cosh(arg.t)); - return Rall1d(v,sinh(arg.t)*arg.grad); - } - -template -INLINE Rall1d sqr(const Rall1d& arg) - { - T v; - v=(arg.t*arg.t); - return Rall1d(v,(2.0*arg.t)*arg.grad); - } - -template -INLINE Rall1d pow(const Rall1d& arg,double m) - { - T v; - v=(pow(arg.t,m)); - return Rall1d(v,(m*v/arg.t)*arg.grad); - } - -template -INLINE Rall1d sqrt(const Rall1d& arg) - { - T v; - v=sqrt(arg.t); - return Rall1d(v, (0.5/v)*arg.grad); - } - -template -INLINE Rall1d atan(const Rall1d& x) +template +INLINE Rall1d operator/(const Rall1d &lhs, const Rall1d &rhs) { - T v; - v=(atan(x.t)); - return Rall1d(v,x.grad/(1.0+sqr(x.t))); + return Rall1d(lhs.t / rhs.t, (lhs.grad * rhs.t - lhs.t * rhs.grad) / (rhs.t * rhs.t)); } -template -INLINE Rall1d hypot(const Rall1d& y,const Rall1d& x) +template +INLINE Rall1d operator*(const Rall1d &lhs, const Rall1d &rhs) { - T v; - v=(hypot(y.t,x.t)); - return Rall1d(v,(x.t/v)*x.grad+(y.t/v)*y.grad); + return Rall1d(lhs.t * rhs.t, rhs.t * lhs.grad + lhs.t * rhs.grad); } -template -INLINE Rall1d asin(const Rall1d& x) +template +INLINE Rall1d operator+(const Rall1d &lhs, const Rall1d &rhs) { - T v; - v=(asin(x.t)); - return Rall1d(v,x.grad/sqrt(1.0-sqr(x.t))); + return Rall1d(lhs.t + rhs.t, lhs.grad + rhs.grad); } -template -INLINE Rall1d acos(const Rall1d& x) +template +INLINE Rall1d operator-(const Rall1d &lhs, const Rall1d &rhs) { - T v; - v=(acos(x.t)); - return Rall1d(v,-x.grad/sqrt(1.0-sqr(x.t))); + return Rall1d(lhs.t - rhs.t, lhs.grad - rhs.grad); } -template -INLINE Rall1d abs(const Rall1d& x) +template INLINE Rall1d operator-(const Rall1d &arg) { - T v; - v=(Sign(x)); - return Rall1d(v*x,v*x.grad); + return Rall1d(-arg.t, -arg.grad); } - -template -INLINE S Norm(const Rall1d& value) +template INLINE Rall1d operator*(S s, const Rall1d &v) { - return Norm(value.t); + return Rall1d(s * v.t, s * v.grad); } -template -INLINE Rall1d tanh(const Rall1d& arg) -{ - T v(tanh(arg.t)); - return Rall1d(v,arg.grad/sqr(cosh(arg.t))); -} - -template -INLINE Rall1d atan2(const Rall1d& y,const Rall1d& x) +template INLINE Rall1d operator*(const Rall1d &v, S s) { - T v(x.t*x.t+y.t*y.t); - return Rall1d(atan2(y.t,x.t),(x.t*y.grad-y.t*x.grad)/v); + return Rall1d(v.t * s, v.grad * s); } - -template -INLINE Rall1d LinComb(S alfa,const Rall1d& a, - const T& beta,const Rall1d& b ) { - return Rall1d( - LinComb(alfa,a.t,beta,b.t), - LinComb(alfa,a.grad,beta,b.grad) - ); -} - -template -INLINE void LinCombR(S alfa,const Rall1d& a, - const T& beta,const Rall1d& b,Rall1d& result ) { - LinCombR(alfa, a.t, beta, b.t, result.t); - LinCombR(alfa, a.grad, beta, b.grad, result.grad); -} - - -template -INLINE void SetToZero(Rall1d& value) - { - SetToZero(value.grad); - SetToZero(value.t); - } -template -INLINE void SetToIdentity(Rall1d& value) - { - SetToIdentity(value.t); - SetToZero(value.grad); - } - -template -INLINE bool Equal(const Rall1d& y,const Rall1d& x,double eps=epsilon) +template INLINE Rall1d operator+(S s, const Rall1d &v) { - return (Equal(x.t,y.t,eps)&&Equal(x.grad,y.grad,eps)); + return Rall1d(s + v.t, v.grad); } +template INLINE Rall1d operator+(const Rall1d &v, S s) +{ + return Rall1d(v.t + s, v.grad); } +template INLINE Rall1d operator-(S s, const Rall1d &v) +{ + return Rall1d(s - v.t, -v.grad); +} +template INLINE Rall1d operator-(const Rall1d &v, S s) +{ + return Rall1d(v.t - s, v.grad); +} + +template INLINE Rall1d operator/(S s, const Rall1d &v) +{ + return Rall1d(s / v.t, (-s * v.grad) / (v.t * v.t)); +} + +template INLINE Rall1d operator/(const Rall1d &v, S s) +{ + return Rall1d(v.t / s, v.grad / s); +} + +template INLINE Rall1d exp(const Rall1d &arg) +{ + T v; + v = (exp(arg.t)); + return Rall1d(v, v * arg.grad); +} + +template INLINE Rall1d log(const Rall1d &arg) +{ + T v; + v = (log(arg.t)); + return Rall1d(v, arg.grad / arg.t); +} + +template INLINE Rall1d sin(const Rall1d &arg) +{ + T v; + v = (sin(arg.t)); + return Rall1d(v, cos(arg.t) * arg.grad); +} + +template INLINE Rall1d cos(const Rall1d &arg) +{ + T v; + v = (cos(arg.t)); + return Rall1d(v, -sin(arg.t) * arg.grad); +} + +template INLINE Rall1d tan(const Rall1d &arg) +{ + T v; + v = (tan(arg.t)); + return Rall1d(v, arg.grad / sqr(cos(arg.t))); +} + +template INLINE Rall1d sinh(const Rall1d &arg) +{ + T v; + v = (sinh(arg.t)); + return Rall1d(v, cosh(arg.t) * arg.grad); +} + +template INLINE Rall1d cosh(const Rall1d &arg) +{ + T v; + v = (cosh(arg.t)); + return Rall1d(v, sinh(arg.t) * arg.grad); +} + +template INLINE Rall1d sqr(const Rall1d &arg) +{ + T v; + v = (arg.t * arg.t); + return Rall1d(v, (2.0 * arg.t) * arg.grad); +} + +template +INLINE Rall1d pow(const Rall1d &arg, double m) +{ + T v; + v = (pow(arg.t, m)); + return Rall1d(v, (m * v / arg.t) * arg.grad); +} + +template INLINE Rall1d sqrt(const Rall1d &arg) +{ + T v; + v = sqrt(arg.t); + return Rall1d(v, (0.5 / v) * arg.grad); +} + +template INLINE Rall1d atan(const Rall1d &x) +{ + T v; + v = (atan(x.t)); + return Rall1d(v, x.grad / (1.0 + sqr(x.t))); +} + +template +INLINE Rall1d hypot(const Rall1d &y, const Rall1d &x) +{ + T v; + v = (hypot(y.t, x.t)); + return Rall1d(v, (x.t / v) * x.grad + (y.t / v) * y.grad); +} + +template INLINE Rall1d asin(const Rall1d &x) +{ + T v; + v = (asin(x.t)); + return Rall1d(v, x.grad / sqrt(1.0 - sqr(x.t))); +} + +template INLINE Rall1d acos(const Rall1d &x) +{ + T v; + v = (acos(x.t)); + return Rall1d(v, -x.grad / sqrt(1.0 - sqr(x.t))); +} + +template INLINE Rall1d abs(const Rall1d &x) +{ + T v; + v = (Sign(x)); + return Rall1d(v * x, v * x.grad); +} + +template INLINE S Norm(const Rall1d &value) +{ + return Norm(value.t); +} + +template INLINE Rall1d tanh(const Rall1d &arg) +{ + T v(tanh(arg.t)); + return Rall1d(v, arg.grad / sqr(cosh(arg.t))); +} + +template +INLINE Rall1d atan2(const Rall1d &y, const Rall1d &x) +{ + T v(x.t * x.t + y.t * y.t); + return Rall1d(atan2(y.t, x.t), (x.t * y.grad - y.t * x.grad) / v); +} + +template +INLINE Rall1d LinComb(S alfa, + const Rall1d &a, + const T &beta, + const Rall1d &b) +{ + return Rall1d(LinComb(alfa, a.t, beta, b.t), LinComb(alfa, a.grad, beta, b.grad)); +} + +template +INLINE void LinCombR(S alfa, + const Rall1d &a, + const T &beta, + const Rall1d &b, + Rall1d &result) +{ + LinCombR(alfa, a.t, beta, b.t, result.t); + LinCombR(alfa, a.grad, beta, b.grad, result.grad); +} + +template INLINE void SetToZero(Rall1d &value) +{ + SetToZero(value.grad); + SetToZero(value.t); +} +template INLINE void SetToIdentity(Rall1d &value) +{ + SetToIdentity(value.t); + SetToZero(value.grad); +} + +template +INLINE bool Equal(const Rall1d &y, const Rall1d &x, double eps = epsilon) +{ + return (Equal(x.t, y.t, eps) && Equal(x.grad, y.grad, eps)); +} + +} // namespace KDL #endif diff --git a/intern/itasc/kdl/utilities/rall2d.h b/intern/itasc/kdl/utilities/rall2d.h index 72659895d8a..01e757609de 100644 --- a/intern/itasc/kdl/utilities/rall2d.h +++ b/intern/itasc/kdl/utilities/rall2d.h @@ -1,536 +1,518 @@ /***************************************************************************** - * \file - * class for automatic differentiation on scalar values and 1st + * \file + * class for automatic differentiation on scalar values and 1st * derivatives and 2nd derivative. - * + * * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * - * \version + * \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 ! + * 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$ + * - $log$ * * \par Release - * $Name: $ + * $Name: $ ****************************************************************************/ #ifndef Rall2D_H #define Rall2D_H -#include -#include #include "utility.h" - +#include +#include namespace KDL { /** - * Rall2d contains a value, and its gradient and its 2nd derivative, and defines an algebraic + * 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. + * - 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,.. + * S is usefull when you recurse a Rall1d object into itself to create a 2nd, 3th, 4th,.. * derivatives. (e.g. Rall1d< Rall1d, Rall1d, double> ). * - * S is always passed by value. + * S is always passed by value. * * \par Class Type * Concrete implementation */ -template -class Rall2d - { - public : - T t; //!< value - V d; //!< 1st derivative - V dd; //!< 2nd derivative - public : - // = Constructors - INLINE Rall2d() {} +template class Rall2d { + public: + T t; //!< value + V d; //!< 1st derivative + V dd; //!< 2nd derivative + public: + // = Constructors + INLINE Rall2d() {} - explicit INLINE Rall2d(typename TI::Arg c) - {t=c;SetToZero(d);SetToZero(dd);} + explicit INLINE Rall2d(typename TI::Arg c) + { + t = c; + SetToZero(d); + SetToZero(dd); + } - INLINE Rall2d(typename TI::Arg tn,const V& afg):t(tn),d(afg) {SetToZero(dd);} + INLINE Rall2d(typename TI::Arg tn, const V &afg) : t(tn), d(afg) + { + SetToZero(dd); + } - INLINE Rall2d(typename TI::Arg tn,const V& afg,const V& afg2):t(tn),d(afg),dd(afg2) {} + INLINE Rall2d(typename TI::Arg tn, const V &afg, const V &afg2) : t(tn), d(afg), dd(afg2) {} - // = Copy Constructor - INLINE Rall2d(const Rall2d& 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; - } + // = Copy Constructor + INLINE Rall2d(const Rall2d &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. - INLINE V& D() { - return d; - } + // = Member functions to access internal structures : + INLINE T &Value() + { + return t; + } - INLINE V& DD() { - return dd; - } - INLINE static Rall2d Zero() { - Rall2d tmp; - SetToZero(tmp); - return tmp; - } - INLINE static Rall2d Identity() { - Rall2d tmp; - SetToIdentity(tmp); - return tmp; - } + INLINE V &D() + { + return d; + } - // = assignment operators - INLINE Rall2d& operator =(S c) - {t=c;SetToZero(d);SetToZero(dd);return *this;} + INLINE V &DD() + { + return dd; + } + INLINE static Rall2d Zero() + { + Rall2d tmp; + SetToZero(tmp); + return tmp; + } + INLINE static Rall2d Identity() + { + Rall2d tmp; + SetToIdentity(tmp); + return tmp; + } - INLINE Rall2d& operator =(const Rall2d& r) - {t=r.t;d=r.d;dd=r.dd;return *this;} + // = assignment operators + INLINE Rall2d &operator=(S c) + { + t = c; + SetToZero(d); + SetToZero(dd); + return *this; + } - INLINE Rall2d& operator /=(const Rall2d& 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 &operator=(const Rall2d &r) + { + t = r.t; + d = r.d; + dd = r.dd; + return *this; + } - INLINE Rall2d& operator *=(const Rall2d& 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 &operator/=(const Rall2d &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& operator +=(const Rall2d& rhs) - { - t +=rhs.t; - d +=rhs.d; - dd+=rhs.dd; - return *this; - } + INLINE Rall2d &operator*=(const Rall2d &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& operator -=(const Rall2d& rhs) - { - t -= rhs.t; - d -= rhs.d; - dd -= rhs.dd; - return *this; - } + INLINE Rall2d &operator+=(const Rall2d &rhs) + { + t += rhs.t; + d += rhs.d; + dd += rhs.dd; + return *this; + } - INLINE Rall2d& operator /=(S rhs) - { - t /= rhs; - d /= rhs; - dd /= rhs; - return *this; - } + INLINE Rall2d &operator-=(const Rall2d &rhs) + { + t -= rhs.t; + d -= rhs.d; + dd -= rhs.dd; + return *this; + } - INLINE Rall2d& operator *=(S rhs) - { - t *= rhs; - d *= rhs; - dd *= rhs; - return *this; - } + INLINE Rall2d &operator/=(S rhs) + { + t /= rhs; + d /= rhs; + dd /= rhs; + return *this; + } - INLINE Rall2d& operator -=(S rhs) - { - t -= rhs; - return *this; - } + INLINE Rall2d &operator*=(S rhs) + { + t *= rhs; + d *= rhs; + dd *= rhs; + return *this; + } - INLINE Rall2d& operator +=(S rhs) - { - t += rhs; - return *this; - } + INLINE Rall2d &operator-=(S rhs) + { + t -= rhs; + return *this; + } - // = Operators between Rall2d objects -/* - friend INLINE Rall2d operator /(const Rall2d& lhs,const Rall2d& rhs); - friend INLINE Rall2d operator *(const Rall2d& lhs,const Rall2d& rhs); - friend INLINE Rall2d operator +(const Rall2d& lhs,const Rall2d& rhs); - friend INLINE Rall2d operator -(const Rall2d& lhs,const Rall2d& rhs); - friend INLINE Rall2d operator -(const Rall2d& arg); - friend INLINE Rall2d operator *(S s,const Rall2d& v); - friend INLINE Rall2d operator *(const Rall2d& v,S s); - friend INLINE Rall2d operator +(S s,const Rall2d& v); - friend INLINE Rall2d operator +(const Rall2d& v,S s); - friend INLINE Rall2d operator -(S s,const Rall2d& v); - friend INLINE Rall2d operator -(const Rall2d& v,S s); - friend INLINE Rall2d operator /(S s,const Rall2d& v); - friend INLINE Rall2d operator /(const Rall2d& v,S s); - - // = Mathematical functions that operate on Rall2d objects - - friend INLINE Rall2d exp(const Rall2d& arg); - friend INLINE Rall2d log(const Rall2d& arg); - friend INLINE Rall2d sin(const Rall2d& arg); - friend INLINE Rall2d cos(const Rall2d& arg); - friend INLINE Rall2d tan(const Rall2d& arg); - friend INLINE Rall2d sinh(const Rall2d& arg); - friend INLINE Rall2d cosh(const Rall2d& arg); - friend INLINE Rall2d tanh(const Rall2d& arg); - friend INLINE Rall2d sqr(const Rall2d& arg); - friend INLINE Rall2d pow(const Rall2d& arg,double m) ; - friend INLINE Rall2d sqrt(const Rall2d& arg); - friend INLINE Rall2d asin(const Rall2d& arg); - friend INLINE Rall2d acos(const Rall2d& arg); - friend INLINE Rall2d atan(const Rall2d& x); - friend INLINE Rall2d atan2(const Rall2d& y,const Rall2d& x); - friend INLINE Rall2d abs(const Rall2d& x); - friend INLINE Rall2d hypot(const Rall2d& y,const Rall2d& x); - // returns sqrt(y*y+x*x), but is optimized for accuracy and speed. - friend INLINE S Norm(const Rall2d& value) ; - // returns Norm( value.Value() ). - - // = Some utility functions to improve performance - // (should also be declared on primitive types to improve uniformity - friend INLINE Rall2d LinComb(S alfa,const Rall2d& a, - TI::Arg beta,const Rall2d& b ); - friend INLINE void LinCombR(S alfa,const Rall2d& a, - TI::Arg beta,const Rall2d& b,Rall2d& result ); - // = Setting value of a Rall2d object to 0 or 1 - friend INLINE void SetToZero(Rall2d& value); - friend INLINE void SetToOne(Rall2d& value); - // = Equality in an eps-interval - friend INLINE bool Equal(const Rall2d& y,const Rall2d& x,double eps); - */ - }; + INLINE Rall2d &operator+=(S rhs) + { + t += rhs; + return *this; + } + // = Operators between Rall2d objects + /* + friend INLINE Rall2d operator /(const Rall2d& lhs,const Rall2d& + rhs); friend INLINE Rall2d operator *(const Rall2d& lhs,const Rall2d& + rhs); friend INLINE Rall2d operator +(const Rall2d& lhs,const Rall2d& + rhs); friend INLINE Rall2d operator -(const Rall2d& lhs,const Rall2d& + rhs); friend INLINE Rall2d operator -(const Rall2d& arg); friend INLINE + Rall2d operator *(S s,const Rall2d& v); friend INLINE Rall2d operator + *(const Rall2d& v,S s); friend INLINE Rall2d operator +(S s,const Rall2d& + v); friend INLINE Rall2d operator +(const Rall2d& v,S s); friend INLINE + Rall2d operator -(S s,const Rall2d& v); friend INLINE Rall2d operator + -(const Rall2d& v,S s); friend INLINE Rall2d operator /(S s,const Rall2d& + v); friend INLINE Rall2d operator /(const Rall2d& v,S s); + // = Mathematical functions that operate on Rall2d objects + friend INLINE Rall2d exp(const Rall2d& arg); + friend INLINE Rall2d log(const Rall2d& arg); + friend INLINE Rall2d sin(const Rall2d& arg); + friend INLINE Rall2d cos(const Rall2d& arg); + friend INLINE Rall2d tan(const Rall2d& arg); + friend INLINE Rall2d sinh(const Rall2d& arg); + friend INLINE Rall2d cosh(const Rall2d& arg); + friend INLINE Rall2d tanh(const Rall2d& arg); + friend INLINE Rall2d sqr(const Rall2d& arg); + friend INLINE Rall2d pow(const Rall2d& arg,double m) ; + friend INLINE Rall2d sqrt(const Rall2d& arg); + friend INLINE Rall2d asin(const Rall2d& arg); + friend INLINE Rall2d acos(const Rall2d& arg); + friend INLINE Rall2d atan(const Rall2d& x); + friend INLINE Rall2d atan2(const Rall2d& y,const Rall2d& x); + friend INLINE Rall2d abs(const Rall2d& x); + friend INLINE Rall2d hypot(const Rall2d& y,const Rall2d& x); + // returns sqrt(y*y+x*x), but is optimized for accuracy and speed. + friend INLINE S Norm(const Rall2d& value) ; + // returns Norm( value.Value() ). + // = Some utility functions to improve performance + // (should also be declared on primitive types to improve uniformity + friend INLINE Rall2d LinComb(S alfa,const Rall2d& a, + TI::Arg beta,const Rall2d& b ); + friend INLINE void LinCombR(S alfa,const Rall2d& a, + TI::Arg beta,const Rall2d& b,Rall2d& result ); + // = Setting value of a Rall2d object to 0 or 1 + friend INLINE void SetToZero(Rall2d& value); + friend INLINE void SetToOne(Rall2d& value); + // = Equality in an eps-interval + friend INLINE bool Equal(const Rall2d& y,const Rall2d& x,double eps); + */ +}; // = Operators between Rall2d objects -template -INLINE Rall2d operator /(const Rall2d& lhs,const Rall2d& rhs) - { - Rall2d 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 -INLINE Rall2d operator *(const Rall2d& lhs,const Rall2d& rhs) - { - Rall2d 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 -INLINE Rall2d operator +(const Rall2d& lhs,const Rall2d& rhs) - { - return Rall2d(lhs.t+rhs.t,lhs.d+rhs.d,lhs.dd+rhs.dd); - } - -template -INLINE Rall2d operator -(const Rall2d& lhs,const Rall2d& rhs) - { - return Rall2d(lhs.t-rhs.t,lhs.d-rhs.d,lhs.dd-rhs.dd); - } - -template -INLINE Rall2d operator -(const Rall2d& arg) - { - return Rall2d(-arg.t,-arg.d,-arg.dd); - } - -template -INLINE Rall2d operator *(S s,const Rall2d& v) - { - return Rall2d(s*v.t,s*v.d,s*v.dd); - } - -template -INLINE Rall2d operator *(const Rall2d& v,S s) - { - return Rall2d(v.t*s,v.d*s,v.dd*s); - } - -template -INLINE Rall2d operator +(S s,const Rall2d& v) - { - return Rall2d(s+v.t,v.d,v.dd); - } - -template -INLINE Rall2d operator +(const Rall2d& v,S s) - { - return Rall2d(v.t+s,v.d,v.dd); - } - -template -INLINE Rall2d operator -(S s,const Rall2d& v) - { - return Rall2d(s-v.t,-v.d,-v.dd); - } - -template -INLINE Rall2d operator -(const Rall2d& v,S s) - { - return Rall2d(v.t-s,v.d,v.dd); - } - -template -INLINE Rall2d operator /(S s,const Rall2d& rhs) - { - Rall2d 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 +INLINE Rall2d operator/(const Rall2d &lhs, const Rall2d &rhs) +{ + Rall2d 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 -INLINE Rall2d operator /(const Rall2d& v,S s) - { - return Rall2d(v.t/s,v.d/s,v.dd/s); - } - - -template -INLINE Rall2d exp(const Rall2d& arg) - { - Rall2d 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 -INLINE Rall2d log(const Rall2d& arg) - { - Rall2d 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 -INLINE Rall2d sin(const Rall2d& arg) - { - T v1 = sin(arg.t); - T v2 = cos(arg.t); - return Rall2d(v1,v2*arg.d,v2*arg.dd - (v1*arg.d)*arg.d ); - } - -template -INLINE Rall2d cos(const Rall2d& arg) - { - T v1 = cos(arg.t); - T v2 = -sin(arg.t); - return Rall2d(v1,v2*arg.d, v2*arg.dd - (v1*arg.d)*arg.d); - } - -template -INLINE Rall2d tan(const Rall2d& arg) - { - T v1 = tan(arg.t); - T v2 = S(1)+sqr(v1); - return Rall2d(v1,v2*arg.d, v2*(arg.dd+(S(2)*v1*sqr(arg.d)))); - } - -template -INLINE Rall2d sinh(const Rall2d& arg) - { - T v1 = sinh(arg.t); - T v2 = cosh(arg.t); - return Rall2d(v1,v2*arg.d,v2*arg.dd + (v1*arg.d)*arg.d ); - } - -template -INLINE Rall2d cosh(const Rall2d& arg) - { - T v1 = cosh(arg.t); - T v2 = sinh(arg.t); - return Rall2d(v1,v2*arg.d,v2*arg.dd + (v1*arg.d)*arg.d ); - } - -template -INLINE Rall2d tanh(const Rall2d& arg) - { - T v1 = tanh(arg.t); - T v2 = S(1)-sqr(v1); - return Rall2d(v1,v2*arg.d, v2*(arg.dd-(S(2)*v1*sqr(arg.d)))); - } - -template -INLINE Rall2d sqr(const Rall2d& arg) - { - return Rall2d(arg.t*arg.t, - (S(2)*arg.t)*arg.d, - S(2)*(sqr(arg.d)+arg.t*arg.dd) - ); - } - -template -INLINE Rall2d pow(const Rall2d& arg,double m) - { - Rall2d 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 -INLINE Rall2d sqrt(const Rall2d& arg) - { - /* By inversion of sqr(x) :*/ - Rall2d 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 -INLINE Rall2d asin(const Rall2d& arg) +template +INLINE Rall2d operator*(const Rall2d &lhs, const Rall2d &rhs) { - /* By inversion of sin(x) */ - Rall2d 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; + Rall2d 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 -INLINE Rall2d acos(const Rall2d& arg) +template +INLINE Rall2d operator+(const Rall2d &lhs, const Rall2d &rhs) { - /* By inversion of cos(x) */ - Rall2d 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; - + return Rall2d(lhs.t + rhs.t, lhs.d + rhs.d, lhs.dd + rhs.dd); } -template -INLINE Rall2d atan(const Rall2d& x) +template +INLINE Rall2d operator-(const Rall2d &lhs, const Rall2d &rhs) { - /* By inversion of tan(x) */ - Rall2d 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; + return Rall2d(lhs.t - rhs.t, lhs.d - rhs.d, lhs.dd - rhs.dd); } -template -INLINE Rall2d atan2(const Rall2d& y,const Rall2d& x) +template INLINE Rall2d operator-(const Rall2d &arg) { - Rall2d 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; + return Rall2d(-arg.t, -arg.d, -arg.dd); } -template -INLINE Rall2d abs(const Rall2d& x) +template INLINE Rall2d operator*(S s, const Rall2d &v) { - T v(Sign(x)); - return Rall2d(v*x,v*x.d,v*x.dd); + return Rall2d(s * v.t, s * v.d, s * v.dd); } -template -INLINE Rall2d hypot(const Rall2d& y,const Rall2d& x) +template INLINE Rall2d operator*(const Rall2d &v, S s) { - Rall2d 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; + return Rall2d(v.t * s, v.d * s, v.dd * s); +} + +template INLINE Rall2d operator+(S s, const Rall2d &v) +{ + return Rall2d(s + v.t, v.d, v.dd); +} + +template INLINE Rall2d operator+(const Rall2d &v, S s) +{ + return Rall2d(v.t + s, v.d, v.dd); +} + +template INLINE Rall2d operator-(S s, const Rall2d &v) +{ + return Rall2d(s - v.t, -v.d, -v.dd); +} + +template INLINE Rall2d operator-(const Rall2d &v, S s) +{ + return Rall2d(v.t - s, v.d, v.dd); +} + +template +INLINE Rall2d operator/(S s, const Rall2d &rhs) +{ + Rall2d 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 INLINE Rall2d operator/(const Rall2d &v, S s) +{ + return Rall2d(v.t / s, v.d / s, v.dd / s); +} + +template INLINE Rall2d exp(const Rall2d &arg) +{ + Rall2d 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 INLINE Rall2d log(const Rall2d &arg) +{ + Rall2d 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 INLINE Rall2d sin(const Rall2d &arg) +{ + T v1 = sin(arg.t); + T v2 = cos(arg.t); + return Rall2d(v1, v2 * arg.d, v2 * arg.dd - (v1 * arg.d) * arg.d); +} + +template INLINE Rall2d cos(const Rall2d &arg) +{ + T v1 = cos(arg.t); + T v2 = -sin(arg.t); + return Rall2d(v1, v2 * arg.d, v2 * arg.dd - (v1 * arg.d) * arg.d); +} + +template INLINE Rall2d tan(const Rall2d &arg) +{ + T v1 = tan(arg.t); + T v2 = S(1) + sqr(v1); + return Rall2d(v1, v2 * arg.d, v2 * (arg.dd + (S(2) * v1 * sqr(arg.d)))); +} + +template INLINE Rall2d sinh(const Rall2d &arg) +{ + T v1 = sinh(arg.t); + T v2 = cosh(arg.t); + return Rall2d(v1, v2 * arg.d, v2 * arg.dd + (v1 * arg.d) * arg.d); +} + +template INLINE Rall2d cosh(const Rall2d &arg) +{ + T v1 = cosh(arg.t); + T v2 = sinh(arg.t); + return Rall2d(v1, v2 * arg.d, v2 * arg.dd + (v1 * arg.d) * arg.d); +} + +template INLINE Rall2d tanh(const Rall2d &arg) +{ + T v1 = tanh(arg.t); + T v2 = S(1) - sqr(v1); + return Rall2d(v1, v2 * arg.d, v2 * (arg.dd - (S(2) * v1 * sqr(arg.d)))); +} + +template INLINE Rall2d sqr(const Rall2d &arg) +{ + return Rall2d( + arg.t * arg.t, (S(2) * arg.t) * arg.d, S(2) * (sqr(arg.d) + arg.t * arg.dd)); +} + +template +INLINE Rall2d pow(const Rall2d &arg, double m) +{ + Rall2d 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 INLINE Rall2d sqrt(const Rall2d &arg) +{ + /* By inversion of sqr(x) :*/ + Rall2d 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 INLINE Rall2d asin(const Rall2d &arg) +{ + /* By inversion of sin(x) */ + Rall2d 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 INLINE Rall2d acos(const Rall2d &arg) +{ + /* By inversion of cos(x) */ + Rall2d 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 INLINE Rall2d atan(const Rall2d &x) +{ + /* By inversion of tan(x) */ + Rall2d 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 +INLINE Rall2d atan2(const Rall2d &y, const Rall2d &x) +{ + Rall2d 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 INLINE Rall2d abs(const Rall2d &x) +{ + T v(Sign(x)); + return Rall2d(v * x, v * x.d, v * x.dd); +} + +template +INLINE Rall2d hypot(const Rall2d &y, const Rall2d &x) +{ + Rall2d 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 -INLINE S Norm(const Rall2d& value) +template INLINE S Norm(const Rall2d &value) { - return Norm(value.t); + return Norm(value.t); } // returns Norm( value.Value() ). - // (should also be declared on primitive types to improve uniformity -template -INLINE Rall2d LinComb(S alfa,const Rall2d& a, - const T& beta,const Rall2d& b ) { - return Rall2d( - LinComb(alfa,a.t,beta,b.t), - LinComb(alfa,a.d,beta,b.d), - LinComb(alfa,a.dd,beta,b.dd) - ); -} - -template -INLINE void LinCombR(S alfa,const Rall2d& a, - const T& beta,const Rall2d& b,Rall2d& 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 -INLINE void SetToZero(Rall2d& value) - { - SetToZero(value.t); - SetToZero(value.d); - SetToZero(value.dd); - } - -template -INLINE void SetToIdentity(Rall2d& value) - { - SetToZero(value.d); - SetToIdentity(value.t); - SetToZero(value.dd); - } - -template -INLINE bool Equal(const Rall2d& y,const Rall2d& x,double eps=epsilon) +template +INLINE Rall2d LinComb(S alfa, + const Rall2d &a, + const T &beta, + const Rall2d &b) { - return (Equal(x.t,y.t,eps)&& - Equal(x.d,y.d,eps)&& - Equal(x.dd,y.dd,eps) - ); + return Rall2d(LinComb(alfa, a.t, beta, b.t), + LinComb(alfa, a.d, beta, b.d), + LinComb(alfa, a.dd, beta, b.dd)); } - +template +INLINE void LinCombR(S alfa, + const Rall2d &a, + const T &beta, + const Rall2d &b, + Rall2d &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 INLINE void SetToZero(Rall2d &value) +{ + SetToZero(value.t); + SetToZero(value.d); + SetToZero(value.dd); +} + +template INLINE void SetToIdentity(Rall2d &value) +{ + SetToZero(value.d); + SetToIdentity(value.t); + SetToZero(value.dd); +} + +template +INLINE bool Equal(const Rall2d &y, const Rall2d &x, double eps = epsilon) +{ + return (Equal(x.t, y.t, eps) && Equal(x.d, y.d, eps) && Equal(x.dd, y.dd, eps)); +} + +} // namespace KDL #endif diff --git a/intern/itasc/kdl/utilities/svd_eigen_HH.hpp b/intern/itasc/kdl/utilities/svd_eigen_HH.hpp index f4b46963813..db2c2d0db5e 100644 --- a/intern/itasc/kdl/utilities/svd_eigen_HH.hpp +++ b/intern/itasc/kdl/utilities/svd_eigen_HH.hpp @@ -19,291 +19,310 @@ // 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 +// Based on the svd of the KDL-0.2 library by Erwin Aertbelien #ifndef SVD_EIGEN_HH_HPP #define SVD_EIGEN_HH_HPP - #include #include -namespace KDL +namespace KDL { +template inline Scalar PYTHAG(Scalar a, Scalar b) { - template 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)); - } - } + 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 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(mxn) - * @param U matrix(mxn) - * @param S vector n - * @param V matrix(nxn) - * @param tmp vector n - * @param maxiter defaults to 150 - * - * @return -2 if maxiter exceeded, 0 otherwise - */ - template - int svd_eigen_HH( - const Eigen::MatrixBase& A, - Eigen::MatrixBase& U, - Eigen::MatrixBase& S, - Eigen::MatrixBase& V, - Eigen::MatrixBase& 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 maxarg2 ? maxarg1 : maxarg2; - } - /* Accumulation of right-hand transformations. */ - for (i=cols-1;i>=0;i--) { - if (i=0;i--) { - ppi=i+1; - g=S(i); - for (j=ppi;j=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 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); - } - + } } + +template 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(mxn) + * @param U matrix(mxn) + * @param S vector n + * @param V matrix(nxn) + * @param tmp vector n + * @param maxiter defaults to 150 + * + * @return -2 if maxiter exceeded, 0 otherwise + */ +template +int svd_eigen_HH(const Eigen::MatrixBase &A, + Eigen::MatrixBase &U, + Eigen::MatrixBase &S, + Eigen::MatrixBase &V, + Eigen::MatrixBase &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); +} + +} // namespace KDL #endif diff --git a/intern/itasc/kdl/utilities/traits.h b/intern/itasc/kdl/utilities/traits.h index 32cc2eaa2e3..0225b49981f 100644 --- a/intern/itasc/kdl/utilities/traits.h +++ b/intern/itasc/kdl/utilities/traits.h @@ -1,114 +1,98 @@ /** \file itasc/kdl/utilities/traits.h * \ingroup intern_itasc */ -#ifndef KDLPV_TRAITS_H -#define KDLPV_TRAITS_H +#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; -} - +class Frame; +class Rotation; +class Vector; +class Twist; +class Wrench; +class FrameVel; +class RotationVel; +class VectorVel; +class TwistVel; +} // namespace KDL /** * @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 + * 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; + Rotation R; Traits dR; \endverbatim */ -template -struct Traits { - typedef T valueType; - typedef T derivType; +template struct Traits { + typedef T valueType; + typedef T derivType; }; -template <> -struct Traits { - typedef KDL::Frame valueType; - typedef KDL::Twist derivType; +template<> struct Traits { + typedef KDL::Frame valueType; + typedef KDL::Twist derivType; }; -template <> -struct Traits { - typedef KDL::Twist valueType; - typedef KDL::Twist derivType; +template<> struct Traits { + typedef KDL::Twist valueType; + typedef KDL::Twist derivType; }; -template <> -struct Traits { - typedef KDL::Wrench valueType; - typedef KDL::Wrench derivType; +template<> struct Traits { + typedef KDL::Wrench valueType; + typedef KDL::Wrench derivType; }; -template <> -struct Traits { - typedef KDL::Rotation valueType; - typedef KDL::Vector derivType; +template<> struct Traits { + typedef KDL::Rotation valueType; + typedef KDL::Vector derivType; }; -template <> -struct Traits { - typedef KDL::Vector valueType; - typedef KDL::Vector derivType; +template<> struct Traits { + typedef KDL::Vector valueType; + typedef KDL::Vector derivType; }; -template <> -struct Traits { - typedef double valueType; - typedef double derivType; +template<> struct Traits { + typedef double valueType; + typedef double derivType; }; -template <> -struct Traits { - typedef float valueType; - typedef float derivType; +template<> struct Traits { + typedef float valueType; + typedef float derivType; }; -template <> -struct Traits { - typedef KDL::Frame valueType; - typedef KDL::TwistVel derivType; +template<> struct Traits { + typedef KDL::Frame valueType; + typedef KDL::TwistVel derivType; }; -template <> -struct Traits { - typedef KDL::Twist valueType; - typedef KDL::TwistVel derivType; +template<> struct Traits { + typedef KDL::Twist valueType; + typedef KDL::TwistVel derivType; }; -template <> -struct Traits { - typedef KDL::Rotation valueType; - typedef KDL::VectorVel derivType; +template<> struct Traits { + typedef KDL::Rotation valueType; + typedef KDL::VectorVel derivType; }; -template <> -struct Traits { - typedef KDL::Vector valueType; - typedef KDL::VectorVel derivType; +template<> struct Traits { + 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 index fa8f89fde63..618ebfb4eca 100644 --- a/intern/itasc/kdl/utilities/utility.cpp +++ b/intern/itasc/kdl/utilities/utility.cpp @@ -3,22 +3,22 @@ */ /** @file utility.cpp * @author Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven - * @version + * @version * ORO_Geometry V0.2 - * + * * @par history * - changed layout of the comments to accommodate doxygen */ - + #include "utility.h" namespace KDL { -int STREAMBUFFERSIZE=10000; +int STREAMBUFFERSIZE = 10000; int MAXLENFILENAME = 255; -const double PI= 3.1415926535897932384626433832795; +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; -} +double epsilon2 = 0.000001 * 0.000001; +} // namespace KDL diff --git a/intern/itasc/kdl/utilities/utility.h b/intern/itasc/kdl/utilities/utility.h index d0e66bade20..39bdd9986c9 100644 --- a/intern/itasc/kdl/utilities/utility.h +++ b/intern/itasc/kdl/utilities/utility.h @@ -1,170 +1,166 @@ -/***************************************************************************** +/***************************************************************************** * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * - * \version + * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release - * $Name: $ + * $Name: $ * \file * Included by most lrl-files to provide some general * functions and macro definitions. - * + * * \par history * - changed layout of the comments to accommodate doxygen */ - #ifndef KDL_UTILITY_H #define KDL_UTILITY_H #include "kdl-config.h" -#include #include #include +#include #ifdef NDEBUG -#undef assert -#define assert(e) ((void)0) +# undef assert +# define assert(e) ((void)0) #endif ///////////////////////////////////////////////////////////// // 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 +# ifdef _MSC_VER +// Microsoft Visual C +# define IMETHOD __forceinline +# else +// Some other compiler, e.g. gcc +# define IMETHOD inline +# endif #else - #define IMETHOD +# 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) +# define FRAMES_CHECKI(a) assert(a) #else - #define FRAMES_CHECKI(a) +# 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; +// so that sin,cos can be overloaded and complete +// resolution of overloaded functions work. +using ::acos; +using ::asin; +using ::atan; +using ::atan2; +using ::cos; +using ::cosh; +using ::exp; +using ::hypot; +using ::log; +using ::pow; +using ::sin; +using ::sinh; +using ::sqrt; +using ::tan; +using ::tanh; #endif #ifndef __GNUC__ - //only real solution : get Rall1d and varia out of namespaces. - #pragma warning (disable:4786) +// 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 +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 TI -{ - public: - typedef const T& Arg; //!< Arg is used for passing the element to a function. +template class TI { + public: + typedef const T &Arg; //!< Arg is used for passing the element to a function. }; -template <> -class TI { -public: - typedef double Arg; +template<> class TI { + public: + typedef double Arg; }; -template <> -class TI { -public: - typedef int Arg; +template<> class TI { + 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 @@ -172,11 +168,11 @@ public: * 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; +//! +extern int STREAMBUFFERSIZE; //! maximal length of a file name -extern int MAXLENFILENAME; +extern int MAXLENFILENAME; //! the value of pi extern const double PI; @@ -188,114 +184,126 @@ extern const double deg2rad; extern const double rad2deg; //! default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001. -extern double epsilon; +extern double epsilon; //! power or 2 of epsilon -extern double epsilon2; +extern double epsilon2; //! the number of derivatives used in the RN-... objects. -extern int VSIZE; - - +extern int VSIZE; #ifndef _MFC_VER -#undef max -inline double max(double a,double b) { - if (btmp)&& (tmp>-eps) ); + 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 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 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 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 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) { +// inline float addDelta(float a,float da,double dt) { // return a+da*dt; -//} - - -} - +// } +} // namespace KDL #endif diff --git a/intern/itasc/kdl/utilities/utility_io.cpp b/intern/itasc/kdl/utilities/utility_io.cpp index de9511b9efb..8e529e75f65 100644 --- a/intern/itasc/kdl/utilities/utility_io.cpp +++ b/intern/itasc/kdl/utilities/utility_io.cpp @@ -4,206 +4,204 @@ /***************************************************************************** * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * - * \version + * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release - * $Name: $ + * $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 #include +#include #include namespace KDL { // -// _functions are private functions +// _functions are private functions // - void _check_istream(std::istream& is) - { - if ((!is.good())&&(is.eof()) ) - { - throw Error_BasicIO_File(); - } - } +void _check_istream(std::istream &is) +{ + if ((!is.good()) && (is.eof())) { + throw Error_BasicIO_File(); + } +} // Eats until the end of the line -static 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; +static 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 -static 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; +static int _EatUntilEndOfComment(std::istream &is, int *countp = NULL) +{ + int ch; + int count; + count = 0; + int prevch; + ch = 0; + do { + prevch = ch; ch = is.get(); - return ch; + 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. -static int _EatSpace( std::istream& is,int* countp=NULL) { - int ch; - int count; - count=-1; - do { - _check_istream(is); +static 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; + 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 whether 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); +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 whether 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(); - } +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) +inline char Upper(char ch) { - /*if (('a'<=ch)&&(ch<='z')) - return (ch-'a'+'A'); - else - return ch; - */ - return toupper(ch); + /*if (('a'<=ch)&&(ch<='z')) + return (ch-'a'+'A'); + else + return ch; + */ + return toupper(ch); } -void Eat(std::istream& is,const char* descript) +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++; + // 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(); + } } - -} - - - -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); + else { + ch = (char)is.get(); + if (chdescr != Upper(ch)) { + throw Error_BasicIO_Unexpected(); + } } - *p=0; - is.putback(ch); + 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); } + +} // namespace KDL diff --git a/intern/itasc/kdl/utilities/utility_io.h b/intern/itasc/kdl/utilities/utility_io.h index 702718818d1..fa277deda78 100644 --- a/intern/itasc/kdl/utilities/utility_io.h +++ b/intern/itasc/kdl/utilities/utility_io.h @@ -1,14 +1,14 @@ -/***************************************************************************** +/***************************************************************************** * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * - * \version + * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release - * $Name: $ + * $Name: $ * * \file utility_io.h * Included by most lrl-files to provide some general @@ -18,38 +18,34 @@ #ifndef KDL_UTILITY_IO_H_84822 #define KDL_UTILITY_IO_H_84822 -//#include - +// #include // Standard includes -#include -#include #include - +#include +#include namespace KDL { - /** * checks validity of basic io of is */ -void _check_istream(std::istream& 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 ); +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, + * @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); +void Eat(std::istream &is, const char *descript); /** * Eats a word of the stream delimited by the letters in delim or space(tabs...) @@ -58,20 +54,16 @@ void Eat(std::istream& is,const char* descript); * @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); +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 ); - - - - -} +void EatEnd(std::istream &is, int delim); +} // namespace KDL #endif diff --git a/intern/itasc/ublas_types.hpp b/intern/itasc/ublas_types.hpp index f8090973570..263b3652177 100644 --- a/intern/itasc/ublas_types.hpp +++ b/intern/itasc/ublas_types.hpp @@ -9,75 +9,78 @@ #ifndef UBLAS_TYPES_HPP_ #define UBLAS_TYPES_HPP_ -#include -#include -#include -#include -#include "kdl/frames.hpp" -#include "kdl/tree.hpp" #include "kdl/chain.hpp" +#include "kdl/frames.hpp" #include "kdl/jacobian.hpp" #include "kdl/jntarray.hpp" +#include "kdl/tree.hpp" +#include +#include +#include +#include - -namespace iTaSC{ +namespace iTaSC { namespace ublas = boost::numeric::ublas; -using KDL::Twist; +using KDL::Chain; using KDL::Frame; -using KDL::Joint; using KDL::Inertia; +using KDL::Jacobian; +using KDL::JntArray; +using KDL::Joint; +using KDL::Rotation; +using KDL::Segment; using KDL::SegmentMap; using KDL::Tree; -using KDL::JntArray; -using KDL::Jacobian; -using KDL::Segment; -using KDL::Rotation; +using KDL::Twist; using KDL::Vector; -using KDL::Chain; #define u_scalar double #define u_vector ublas::vector #define u_zero_vector ublas::zero_vector #define u_matrix ublas::matrix -#define u_matrix6 ublas::matrix +#define u_matrix6 ublas::matrix #define u_identity_matrix ublas::identity_matrix #define u_scalar_vector ublas::scalar_vector #define u_zero_matrix ublas::zero_matrix -#define u_vector6 ublas::bounded_vector +#define u_vector6 ublas::bounded_vector -inline static int changeBase(const u_matrix& J_in, const Frame& T, u_matrix& J_out) { +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 Jj_in = column(J_in,j); - ublas::matrix_column 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; + 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 Jj_in = column(J_in, j); + ublas::matrix_column 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& J_in, const Frame& T, ublas::matrix_range& J_out) { +inline static int changeBase(const ublas::matrix_range &J_in, + const Frame &T, + ublas::matrix_range &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 > Jj_in = column(J_in,j); - ublas::matrix_column > 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; + 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> Jj_in = column(J_in, j); + ublas::matrix_column> 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; } -} +} // namespace iTaSC #endif /* UBLAS_TYPES_HPP_ */