griefith/test2
1
0
Fork 0
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

Revert "Cleanup: make format."

Browse Source 
This reverts commit 06b8ddca8f.

Add ".clang-format" which prevents these files from being formatted.

Commit [0] missed adding this file.

[0]: f43da6fc4c
This commit is contained in:
Campbell Barton
2024-09-20 19:46:33 +10:00
parent 06b8ddca8f
commit 77617fce77
82 changed files with 8484 additions and 8921 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
intern/itasc/.clang-format Normal file
View File

@@ -0,0 +1,2 @@
DisableFormat: true
SortIncludes: false

1275
intern/itasc/Armature.cpp
View File

@@ -8,8 +8,8 @@
#include "Armature.hpp"
#include <algorithm>
#include <stdlib.h>
#include <string.h>
#include <stdlib.h>
namespace iTaSC {
@@ -19,141 +19,129 @@ 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
@@ -164,36 +152,35 @@ 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, &timestamp);
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, &timestamp);
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)
@@ -242,606 +229,550 @@ 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; dof < ndof; dof++) {
Joint_struct js(joint.getType(), ndof, (&q_rest)[dof]);
m_joints.push_back(js);
}
m_njoint += ndof;
return true;
Segment segment(joint, f_tip, M);
if (!m_tree.addSegment(segment, segment_name, hook_name))
return false;
int ndof = joint.getNDof();
for (int dof=0; dof<ndof; dof++) {
Joint_struct js(joint.getType(), ndof, (&q_rest)[dof]);
m_joints.push_back(js);
}
m_njoint+=ndof;
return true;
}
bool Armature::getSegment(const std::string &name,
const unsigned int q_size,
const Joint *&p_joint,
double &q_rest,
double &q,
const Frame *&p_tip)
bool Armature::getSegment(const std::string& name, const unsigned int q_size, const Joint* &p_joint, double &q_rest, double &q, const Frame* &p_tip)
{
SegmentMap::const_iterator sit = m_tree.getSegment(name);
if (sit == m_tree.getSegments().end())
return false;
p_joint = &sit->second.segment.getJoint();
if (q_size < p_joint->getNDof())
return false;
p_tip = &sit->second.segment.getFrameToTip();
for (unsigned int dof = 0; dof < p_joint->getNDof(); dof++) {
(&q_rest)[dof] = m_joints[sit->second.q_nr + dof].rest;
(&q)[dof] = m_qKdl[sit->second.q_nr + dof];
}
return true;
SegmentMap::const_iterator sit = m_tree.getSegment(name);
if (sit == m_tree.getSegments().end())
return false;
p_joint = &sit->second.segment.getJoint();
if (q_size < p_joint->getNDof())
return false;
p_tip = &sit->second.segment.getFrameToTip();
for (unsigned int dof=0; dof<p_joint->getNDof(); dof++) {
(&q_rest)[dof] = m_joints[sit->second.q_nr+dof].rest;
(&q)[dof] = m_qKdl[sit->second.q_nr+dof];
}
return true;
}
double Armature::getMaxJointChange()
{
if (!m_finalized)
return 0.0;
double maxJoint = 0.0;
for (unsigned int i = 0; i < m_njoint; i++) {
// this is a very rough calculation, it doesn't work well for spherical joint
double joint = fabs(m_oldqKdl[i] - m_qKdl[i]);
if (maxJoint < joint)
maxJoint = joint;
}
return maxJoint;
if (!m_finalized)
return 0.0;
double maxJoint = 0.0;
for (unsigned int i=0; i<m_njoint; i++) {
// this is a very rough calculation, it doesn't work well for spherical joint
double joint = fabs(m_oldqKdl[i]-m_qKdl[i]);
if (maxJoint < joint)
maxJoint = joint;
}
return maxJoint;
}
double Armature::getMaxEndEffectorChange()
{
if (!m_finalized)
return 0.0;
double maxDelta = 0.0;
double delta;
Twist twist;
for (unsigned int i = 0; i < m_neffector; i++) {
twist = diff(m_effectors[i].pose, m_effectors[i].oldpose);
delta = twist.rot.Norm();
if (delta > maxDelta)
maxDelta = delta;
delta = twist.vel.Norm();
if (delta > maxDelta)
maxDelta = delta;
}
return maxDelta;
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; 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;
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& timestamp)
{
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& timestamp, JointLockCallback& callback)
{
if (!m_finalized)
return false;
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;
// integration and joint limit
// for spherical joint we must use a more sophisticated method
unsigned int q_nr;
double *qdot = m_qdotKdl(0);
double *q = m_qKdl(0);
double *newq = m_newqKdl(0);
double norm, qx, qz, CX, CZ, sx, sz;
bool locked = false;
int unlocked = 0;
for (q_nr=0; q_nr<m_nq; ++q_nr)
qdot[q_nr]=m_qdot[q_nr];
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;
}
for (q_nr=0; q_nr<m_nq; ) {
Joint_struct* joint = &m_joints[q_nr];
if (!joint->locked) {
switch (joint->type) {
case KDL::Joint::Swing:
{
KDL::Rotation base = KDL::Rot(KDL::Vector(q[0],0.0,q[1]));
(base*KDL::Rot(KDL::Vector(qdot[0],0.0,qdot[1])*timestamp.realTimestep)).GetXZRot().GetValue(newq);
if (joint[0].useLimit) {
if (joint[1].useLimit) {
// elliptical limit
sx = sz = 1.0;
qx = newq[0];
qz = newq[1];
// determine in which quadrant we are
if (qx > 0.0 && qz > 0.0) {
CX = joint[0].max;
CZ = joint[1].max;
} else if (qx <= 0.0 && qz > 0.0) {
CX = -joint[0].min;
CZ = joint[1].max;
qx = -qx;
sx = -1.0;
} else if (qx <= 0.0 && qz <= 0.0) {
CX = -joint[0].min;
CZ = -joint[1].min;
qx = -qx;
qz = -qz;
sx = sz = -1.0;
} else {
CX = joint[0].max;
CZ = -joint[0].min;
qz = -qz;
sz = -1.0;
}
if (CX < KDL::epsilon || CZ < KDL::epsilon) {
// quadrant is degenerated
if (qx > CX) {
newq[0] = CX*sx;
joint[0].locked = true;
}
if (qz > CZ) {
newq[1] = CZ*sz;
joint[0].locked = true;
}
} else {
// general case
qx /= CX;
qz /= CZ;
norm = KDL::sqrt(KDL::sqr(qx)+KDL::sqr(qz));
if (norm > 1.0) {
norm = 1.0/norm;
newq[0] = qx*norm*CX*sx;
newq[1] = qz*norm*CZ*sz;
joint[0].locked = true;
}
}
} else {
// limit on X only
qx = newq[0];
if (qx > joint[0].max) {
newq[0] = joint[0].max;
joint[0].locked = true;
} else if (qx < joint[0].min) {
newq[0] = joint[0].min;
joint[0].locked = true;
}
}
} else if (joint[1].useLimit) {
// limit on Z only
qz = newq[1];
if (qz > joint[1].max) {
newq[1] = joint[1].max;
joint[0].locked = true;
} else if (qz < joint[1].min) {
newq[1] = joint[1].min;
joint[0].locked = true;
}
}
if (joint[0].locked) {
// check the difference from previous position
locked = true;
norm = KDL::sqr(newq[0]-q[0])+KDL::sqr(newq[1]-q[1]);
if (norm < KDL::epsilon2) {
// joint didn't move, no need to update the jacobian
callback.lockJoint(q_nr, 2);
} else {
// joint moved, compute the corresponding velocity
double deltaq[2];
(base.Inverse()*KDL::Rot(KDL::Vector(newq[0],0.0,newq[1]))).GetXZRot().GetValue(deltaq);
deltaq[0] /= timestamp.realTimestep;
deltaq[1] /= timestamp.realTimestep;
callback.lockJoint(q_nr, 2, deltaq);
// no need to update the other joints, it will be done after next rerun
goto end_loop;
}
} else
unlocked++;
break;
}
case KDL::Joint::Sphere:
{
(KDL::Rot(KDL::Vector(q))*KDL::Rot(KDL::Vector(qdot)*timestamp.realTimestep)).GetRot().GetValue(newq);
// no limit on this joint
unlocked++;
break;
}
default:
for (unsigned int i=0; i<joint->ndof; i++) {
newq[i] = q[i]+qdot[i]*timestamp.realTimestep;
if (joint[i].useLimit) {
if (newq[i] > joint[i].max) {
newq[i] = joint[i].max;
joint[0].locked = true;
} else if (newq[i] < joint[i].min) {
newq[i] = joint[i].min;
joint[0].locked = true;
}
}
}
if (joint[0].locked) {
locked = true;
norm = 0.0;
// compute delta to locked position
for (unsigned int i=0; i<joint->ndof; i++) {
qdot[i] = newq[i] - q[i];
norm += qdot[i]*qdot[i];
}
if (norm < KDL::epsilon2) {
// joint didn't move, no need to update the jacobian
callback.lockJoint(q_nr, joint->ndof);
} else {
// solver needs velocity, compute equivalent velocity
for (unsigned int i=0; i<joint->ndof; i++) {
qdot[i] /= timestamp.realTimestep;
}
callback.lockJoint(q_nr, joint->ndof, qdot);
goto end_loop;
}
} else
unlocked++;
}
}
qdot += joint->ndof;
q += joint->ndof;
newq += joint->ndof;
q_nr += joint->ndof;
}
end_loop:
// check if there any other unlocked joint
for (; q_nr < m_nq;) {
Joint_struct *joint = &m_joints[q_nr];
if (!joint->locked)
unlocked++;
q_nr += joint->ndof;
}
// if all joints have been locked no need to run the solver again
return (unlocked) ? locked : false;
// 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& timestamp){
// 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; ee < m_nee; ee++) {
m_fksolver->JntToCart(m_qKdl, m_effectors[ee].pose, m_effectors[ee].name, m_root);
m_jacsolver->JntToJac(m_qKdl, *m_jac, m_effectors[ee].name);
// get the jacobian for the base point, to prepare transformation to world reference
changeRefPoint(*m_jac, -m_effectors[ee].pose.p, *m_jac);
// copy to Jq:
e_matrix &Jq = m_JqArray[ee];
for (unsigned int i = 0; i < 6; i++) {
for (unsigned int j = 0; j < m_nq; j++)
Jq(i, j) = (*m_jac)(i, j);
}
}
// remember that this object has moved
m_updated = true;
//calculate pose and jacobian
for (unsigned int ee=0; ee<m_nee; ee++) {
m_fksolver->JntToCart(m_qKdl,m_effectors[ee].pose,m_effectors[ee].name,m_root);
m_jacsolver->JntToJac(m_qKdl,*m_jac,m_effectors[ee].name);
// get the jacobian for the base point, to prepare transformation to world reference
changeRefPoint(*m_jac,-m_effectors[ee].pose.p,*m_jac);
//copy to Jq:
e_matrix& Jq = m_JqArray[ee];
for(unsigned int i=0;i<6;i++) {
for(unsigned int j=0;j<m_nq;j++)
Jq(i,j)=(*m_jac)(i,j);
}
}
// remember that this object has moved
m_updated = true;
}
const Frame &Armature::getPose(const unsigned int ee)
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& timestamp)
{
if (!m_finalized)
return;
if (!m_finalized)
return;
if (!timestamp.substep && !timestamp.reiterate && timestamp.interpolate) {
popQ(timestamp.cacheTimestamp);
// popConstraints(timestamp.cacheTimestamp);
}
if (!timestamp.substep) {
// save previous joint state for getMaxJointChange()
memcpy(m_oldqKdl(0), m_qKdl(0), sizeof(double) * m_qKdl.rows());
for (unsigned int i = 0; i < m_neffector; i++) {
m_effectors[i].oldpose = m_effectors[i].pose;
}
}
if (!timestamp.substep && !timestamp.reiterate && timestamp.interpolate) {
popQ(timestamp.cacheTimestamp);
//popConstraints(timestamp.cacheTimestamp);
}
// remove all joint lock
for (JointList::iterator jit = m_joints.begin(); jit != m_joints.end(); ++jit) {
(*jit).locked = false;
}
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;
}
}
JointConstraintList::iterator it;
unsigned int iConstraint;
// remove all joint lock
for (JointList::iterator jit=m_joints.begin(); jit!=m_joints.end(); ++jit) {
(*jit).locked = false;
}
// 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);
}
}
JointConstraintList::iterator it;
unsigned int iConstraint;
// scan through the constraints and call the callback functions
for (iConstraint=0, it=m_constraints.begin(); it!=m_constraints.end(); it++, iConstraint++) {
JointConstraint_struct* pConstraint = *it;
unsigned int nr, i;
for (i=0, nr = pConstraint->segment->second.q_nr; i<pConstraint->v_nr; i++, nr++) {
*(double *)&pConstraint->value[i].y = m_qKdl[nr];
*(double *)&pConstraint->value[i].ydot = m_qdotKdl[nr];
}
if (pConstraint->function && (pConstraint->substep || (!timestamp.reiterate && !timestamp.substep))) {
(*pConstraint->function)(timestamp, pConstraint->values, pConstraint->v_nr, pConstraint->param);
}
// recompute the weight in any case, that's the most likely modification
for (i=0, nr=pConstraint->y_nr; i<pConstraint->v_nr; i++, nr++) {
m_Wy(nr) = pConstraint->values[i].alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/;
m_ydot(nr)=pConstraint->value[i].yddot+pConstraint->values[i].feedback*(pConstraint->value[i].yd-pConstraint->value[i].y);
}
}
}
bool Armature::setControlParameter(unsigned int constraintId,
unsigned int valueId,
ConstraintAction action,
double value,
double timestep)
bool Armature::setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, double value, double timestep)
{
unsigned int lastid, i;
if (constraintId == CONSTRAINT_ID_ALL) {
constraintId = 0;
lastid = m_nconstraint;
}
else if (constraintId < m_nconstraint) {
lastid = constraintId + 1;
}
else {
return false;
}
for (; constraintId < lastid; ++constraintId) {
JointConstraint_struct *pConstraint = m_constraints[constraintId];
if (valueId == ID_JOINT) {
for (i = 0; i < pConstraint->v_nr; i++) {
switch (action) {
case ACT_TOLERANCE:
pConstraint->values[i].tolerance = value;
break;
case ACT_FEEDBACK:
pConstraint->values[i].feedback = value;
break;
case ACT_ALPHA:
pConstraint->values[i].alpha = value;
break;
default:
break;
}
}
}
else {
for (i = 0; i < pConstraint->v_nr; i++) {
if (valueId == pConstraint->value[i].id) {
switch (action) {
case ACT_VALUE:
pConstraint->value[i].yd = value;
break;
case ACT_VELOCITY:
pConstraint->value[i].yddot = value;
break;
case ACT_TOLERANCE:
pConstraint->values[i].tolerance = value;
break;
case ACT_FEEDBACK:
pConstraint->values[i].feedback = value;
break;
case ACT_ALPHA:
pConstraint->values[i].alpha = value;
break;
case ACT_NONE:
break;
}
}
}
}
if (m_finalized) {
for (i = 0; i < pConstraint->v_nr; i++)
m_Wy(pConstraint->y_nr +
i) = pConstraint->values[i]
.alpha /*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/;
}
}
return true;
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

246
intern/itasc/Armature.hpp
View File

@@ -9,168 +9,132 @@
#ifndef ARMATURE_HPP_
#define ARMATURE_HPP_
#include "ConstraintSet.hpp"
#include "ControlledObject.hpp"
#include "kdl/treefksolverpos_recursive.hpp"
#include "ConstraintSet.hpp"
#include "kdl/treejnttojacsolver.hpp"
#include "kdl/treefksolverpos_recursive.hpp"
#include <vector>
namespace iTaSC {
class Armature : public iTaSC::ControlledObject {
public:
Armature();
virtual ~Armature();
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& timestamp, JointLockCallback& callback);
virtual void updateKinematics(const Timestamp& timestamp);
virtual void pushCache(const Timestamp& timestamp);
virtual void updateControlOutput(const Timestamp& timestamp);
virtual bool setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, double value, double timestep=0.0);
virtual void initCache(Cache *_cache);
virtual bool setJointArray(const KDL::JntArray& joints);
virtual const KDL::JntArray& getJointArray();
virtual double getArmLength()
{
return m_armlength;
}
virtual double getArmLength()
{
return m_armlength;
}
struct Effector_struct {
std::string name;
Frame oldpose;
Frame pose;
Effector_struct(const std::string &_name)
{
name = _name;
oldpose = pose = F_identity;
}
};
typedef std::vector<Effector_struct> EffectorList;
struct Effector_struct {
std::string name;
Frame oldpose;
Frame pose;
Effector_struct(const std::string& _name) {name = _name; oldpose = pose = F_identity;}
};
typedef std::vector<Effector_struct> EffectorList;
enum ID {
ID_JOINT = 1,
ID_JOINT_RX = 2,
ID_JOINT_RY = 3,
ID_JOINT_RZ = 4,
ID_JOINT_TX = 2,
ID_JOINT_TY = 3,
ID_JOINT_TZ = 4,
};
struct JointConstraint_struct {
SegmentMap::const_iterator segment;
ConstraintSingleValue value[3];
ConstraintValues values[3];
ConstraintCallback function;
unsigned int v_nr;
unsigned int y_nr; // first coordinate of constraint in Y vector
void *param;
bool freeParam;
bool substep;
JointConstraint_struct(SegmentMap::const_iterator _segment,
unsigned int _y_nr,
ConstraintCallback _function,
void *_param,
bool _freeParam,
bool _substep);
~JointConstraint_struct();
};
typedef std::vector<JointConstraint_struct *> JointConstraintList;
enum ID {
ID_JOINT=1,
ID_JOINT_RX=2,
ID_JOINT_RY=3,
ID_JOINT_RZ=4,
ID_JOINT_TX=2,
ID_JOINT_TY=3,
ID_JOINT_TZ=4,
};
struct JointConstraint_struct {
SegmentMap::const_iterator segment;
ConstraintSingleValue value[3];
ConstraintValues values[3];
ConstraintCallback function;
unsigned int v_nr;
unsigned int y_nr; // first coordinate of constraint in Y vector
void* param;
bool freeParam;
bool substep;
JointConstraint_struct(SegmentMap::const_iterator _segment, unsigned int _y_nr, ConstraintCallback _function, void* _param, bool _freeParam, bool _substep);
~JointConstraint_struct();
};
typedef std::vector<JointConstraint_struct*> JointConstraintList;
struct Joint_struct {
KDL::Joint::JointType type;
unsigned short ndof;
bool useLimit;
bool locked;
double rest;
double min;
double max;
struct Joint_struct {
KDL::Joint::JointType type;
unsigned short ndof;
bool useLimit;
bool locked;
double rest;
double min;
double max;
Joint_struct(KDL::Joint::JointType _type, unsigned int _ndof, double _rest)
: type(_type), ndof(_ndof), rest(_rest)
{
useLimit = locked = false;
min = 0.0;
max = 0.0;
}
};
typedef std::vector<Joint_struct> JointList;
Joint_struct(KDL::Joint::JointType _type, unsigned int _ndof, double _rest) :
type(_type), ndof(_ndof), rest(_rest) { useLimit=locked=false; min=0.0; max=0.0; }
};
typedef std::vector<Joint_struct> JointList;
protected:
virtual void updateJacobian();
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;
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;
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);
void pushQ(CacheTS timestamp);
bool popQ(CacheTS timestamp);
// void pushConstraints(CacheTS timestamp);
// bool popConstraints(CacheTS timestamp);
};
} // namespace iTaSC
}
#endif /* ARMATURE_HPP_ */

1114
intern/itasc/Cache.cpp
View File

@@ -6,647 +6,619 @@
* \ingroup intern_itasc
*/
#include "Cache.hpp"
#include <string.h>
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "Cache.hpp"
namespace iTaSC {
CacheEntry::~CacheEntry()
{
for (unsigned int id = 0; id < m_count; id++)
m_channelArray[id].clear();
if (m_channelArray)
free(m_channelArray);
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; 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;
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; 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;
}
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
}

281
intern/itasc/Cache.hpp
View File

@@ -13,57 +13,51 @@
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) << 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)
#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& timestamp)
{
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.
@@ -74,13 +68,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.
@@ -91,113 +85,108 @@ struct CacheBuffer;
struct CacheItem;
struct CacheChannel;
class Cache {
private:
/* map between device and cache entry.
Dynamically updated when more devices create cache channels */
typedef std::map<const void *, struct CacheEntry *> CacheMap;
CacheMap m_cache;
const CacheItem *getCurrentCacheItemInternal(const void *device, int channel, CacheTS timestamp);
class Cache
{
private:
/* map between device and cache entry.
Dynamically updated when more devices create cache channels */
typedef std::map<const void *, struct CacheEntry*> CacheMap;
CacheMap m_cache;
const CacheItem *getCurrentCacheItemInternal(const void *device, int channel, CacheTS timestamp);
public:
Cache();
~Cache();
/* add a cache channel, maxItemSize must be < 256k.
name : channel name, truncated at 31 characters
msxItemSize : maximum size of item in bytes, items of larger size will be rejected
return value >= 0: channel id, -1: error */
int addChannel(const void *device, const char *name, unsigned int maxItemSize);
public:
Cache();
~Cache();
/* add a cache channel, maxItemSize must be < 256k.
name : channel name, truncated at 31 characters
msxItemSize : maximum size of item in bytes, items of larger size will be rejected
return value >= 0: channel id, -1: error */
int addChannel(const void *device, const char *name, unsigned int maxItemSize);
/* delete a cache channel (and all associated buffers and items) */
int deleteChannel(const void *device, int channel);
/* delete all channels of a device and remove the device from the map */
int deleteDevice(const void *device);
/* removes all cache items, leaving the special item at timestamp=0.
if device=NULL, apply to all devices. */
void clearCacheFrom(const void *device, CacheTS timestamp);
/* delete a cache channel (and all associated buffers and items) */
int deleteChannel(const void *device, int channel);
/* delete all channels of a device and remove the device from the map */
int deleteDevice(const void *device);
/* removes all cache items, leaving the special item at timestamp=0.
if device=NULL, apply to all devices. */
void clearCacheFrom(const void *device, CacheTS timestamp);
/* add a new cache item
channel: the cache channel (as returned by AddChannel
data, length: the cache item and length in bytes
If data is NULL, the memory is allocated in the cache but not writen
return: error: NULL, success: pointer to item in cache */
void *addCacheItem(const void *device, int channel, CacheTS timestamp, void *data, unsigned int length);
/* add a new cache item
channel: the cache channel (as returned by AddChannel
data, length: the cache item and length in bytes
If data is NULL, the memory is allocated in the cache but not writen
return: error: NULL, success: pointer to item in cache */
void *addCacheItem(
const void *device, int channel, CacheTS timestamp, void *data, unsigned int length);
/* specialized function to add a vector of double in the cache
It will first check if a vector exist already in the cache for the same timestamp
and compared the cached vector with the new values.
If all values are within threshold, the vector is updated but the cache is not deleted
for the future timestamps. */
double *addCacheVectorIfDifferent(const void *device, int channel, CacheTS timestamp, double *data, unsigned int length, double threshold);
/* specialized function to add a vector of double in the cache
It will first check if a vector exist already in the cache for the same timestamp
and compared the cached vector with the new values.
If all values are within threshold, the vector is updated but the cache is not deleted
for the future timestamps. */
double *addCacheVectorIfDifferent(const void *device,
int channel,
CacheTS timestamp,
double *data,
unsigned int length,
double threshold);
/* returns the cache item with timestamp that is just before the given timestamp.
returns the data pointer or NULL if there is no cache item before timestamp.
On return, timestamp is updated with the actual timestamp of the item being returned.
Note that the length of the item is not returned, it is up to the device to organize
the data so that length can be retrieved from the data if needed.
Device can NULL, it will then just look the first channel available, useful to
test the status of the cache. */
const void *getPreviousCacheItem(const void *device, int channel, CacheTS *timestamp);
/* returns the cache item with 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);
/* 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
@@ -209,29 +198,31 @@ 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_ */

295
intern/itasc/ConstraintSet.cpp
View File

@@ -11,179 +11,164 @@
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)
{
m_chi += m_chidot * timestamp.realTimestep;
m_externalPose = _external_pose;
// update the internal pose and Jf
updateJacobian();
// check if loop is already closed, if not update the pose and Jf
unsigned int iter = 0;
while (iter < 5 && !closeLoop())
iter++;
}
double ConstraintSet::getMaxTimestep(double &timestep)
void ConstraintSet::modelUpdate(Frame& _external_pose,const Timestamp& timestamp)
{
e_scalar maxChidot = m_chidot.array().abs().maxCoeff();
if (timestep * maxChidot > m_maxDeltaChi) {
timestep = m_maxDeltaChi / maxChidot;
}
return timestep;
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++;
}
bool ConstraintSet::initialise(Frame &init_pose)
double ConstraintSet::getMaxTimestep(double& timestep)
{
m_externalPose = init_pose;
// get current Jf
updateJacobian();
unsigned int iter = 0;
while (iter < m_maxIter && !closeLoop()) {
iter++;
}
if (iter < m_maxIter)
return true;
else
return false;
e_scalar maxChidot = m_chidot.array().abs().maxCoeff();
if (timestep*maxChidot > m_maxDeltaChi) {
timestep = m_maxDeltaChi/maxChidot;
}
return timestep;
}
bool ConstraintSet::setControlParameter(int id,
ConstraintAction action,
double data,
double timestep)
{
ConstraintValues values;
ConstraintSingleValue value;
values.values = &value;
values.number = 0;
values.action = action;
values.id = id;
value.action = action;
value.id = id;
switch (action) {
case ACT_NONE:
return true;
case ACT_VALUE:
value.yd = data;
values.number = 1;
break;
case ACT_VELOCITY:
value.yddot = data;
values.number = 1;
break;
case ACT_TOLERANCE:
values.tolerance = data;
break;
case ACT_FEEDBACK:
values.feedback = data;
break;
case ACT_ALPHA:
values.alpha = data;
break;
default:
assert(action == ACT_NONE);
break;
}
return setControlParameters(&values, 1, timestep);
}
bool ConstraintSet::initialise(Frame& init_pose){
m_externalPose=init_pose;
// get current Jf
updateJacobian();
bool ConstraintSet::closeLoop()
{
// Invert Jf
// TODO: svd_boost_Macie has problems if Jf contains zero-rows
// toggle=!toggle;
// svd_boost_Macie(Jf,U,S,V,B,temp,1e-3*threshold,toggle);
int ret = KDL::svd_eigen_HH(m_Jf, m_U, m_S, m_V, m_temp);
if (ret < 0)
return false;
// the reference point and frame of the jacobian is the base frame
// m_externalPose-m_internalPose is the twist to extend the end effector
// to get the required pose => change the reference point to the base frame
Twist twist_delta(diff(m_internalPose, m_externalPose));
twist_delta = twist_delta.RefPoint(-m_internalPose.p);
for (unsigned int i = 0; i < 6; i++)
m_tdelta(i) = twist_delta(i);
// TODO: use damping in constraintset inversion?
for (unsigned int i = 0; i < 6; i++) {
if (m_S(i) < m_threshold) {
m_B.row(i).setConstant(0.0);
unsigned int iter=0;
while(iter<m_maxIter&&!closeLoop()){
iter++;
}
else {
m_B.row(i) = m_U.col(i) / m_S(i);
}
}
m_Jf_inv.noalias() = m_V * m_B;
m_chi.noalias() += m_Jf_inv * m_tdelta;
updateJacobian();
// m_externalPose-m_internalPose in end effector frame
// this is just to compare the pose, a different formula would work too
return Equal(m_internalPose.Inverse() * m_externalPose, F_identity, m_threshold);
if (iter<m_maxIter)
return true;
else
return false;
}
bool ConstraintSet::setControlParameter(int id, ConstraintAction action, double data, double timestep)
{
ConstraintValues values;
ConstraintSingleValue value;
values.values = &value;
values.number = 0;
values.action = action;
values.id = id;
value.action = action;
value.id = id;
switch (action) {
case ACT_NONE:
return true;
case ACT_VALUE:
value.yd = data;
values.number = 1;
break;
case ACT_VELOCITY:
value.yddot = data;
values.number = 1;
break;
case ACT_TOLERANCE:
values.tolerance = data;
break;
case ACT_FEEDBACK:
values.feedback = data;
break;
case ACT_ALPHA:
values.alpha = data;
break;
default:
assert(action==ACT_NONE);
break;
}
return setControlParameters(&values, 1, timestep);
}
bool ConstraintSet::closeLoop(){
//Invert Jf
//TODO: svd_boost_Macie has problems if Jf contains zero-rows
//toggle=!toggle;
//svd_boost_Macie(Jf,U,S,V,B,temp,1e-3*threshold,toggle);
int ret = KDL::svd_eigen_HH(m_Jf,m_U,m_S,m_V,m_temp);
if(ret<0)
return false;
// the reference point and frame of the jacobian is the base frame
// m_externalPose-m_internalPose is the twist to extend the end effector
// to get the required pose => change the reference point to the base frame
Twist twist_delta(diff(m_internalPose,m_externalPose));
twist_delta=twist_delta.RefPoint(-m_internalPose.p);
for(unsigned int i=0;i<6;i++)
m_tdelta(i)=twist_delta(i);
//TODO: use damping in constraintset inversion?
for(unsigned int i=0;i<6;i++) {
if(m_S(i)<m_threshold){
m_B.row(i).setConstant(0.0);
} else {
m_B.row(i) = m_U.col(i)/m_S(i);
}
}
m_Jf_inv.noalias()=m_V*m_B;
m_chi.noalias()+=m_Jf_inv*m_tdelta;
updateJacobian();
// m_externalPose-m_internalPose in end effector frame
// this is just to compare the pose, a different formula would work too
return Equal(m_internalPose.Inverse()*m_externalPose,F_identity,m_threshold);
}
}
} // namespace iTaSC

187
intern/itasc/ConstraintSet.hpp
View File

@@ -9,143 +9,108 @@
#ifndef CONSTRAINTSET_HPP_
#define CONSTRAINTSET_HPP_
#include "Cache.hpp"
#include "eigen_types.hpp"
#include "kdl/frames.hpp"
#include "eigen_types.hpp"
#include "Cache.hpp"
#include <vector>
namespace iTaSC {
enum ConstraintAction {
ACT_NONE = 0,
ACT_VALUE = 1,
ACT_VELOCITY = 2,
ACT_TOLERANCE = 4,
ACT_FEEDBACK = 8,
ACT_ALPHA = 16
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& timestamp, struct ConstraintValues* const _values, unsigned int _nvalues, void* _param);
class ConstraintSet {
protected:
unsigned int m_nc;
e_scalar m_maxDeltaChi;
e_matrix m_Cf;
e_vector m_Wy, m_y, m_ydot;
e_vector6 m_chi, m_chidot, m_S, m_temp, m_tdelta;
e_matrix6 m_Jf, m_U, m_V, m_B, m_Jf_inv;
KDL::Frame m_internalPose, m_externalPose;
ConstraintCallback m_constraintCallback;
void *m_constraintParam;
void *m_poseParam;
bool m_toggle;
bool m_substep;
double m_threshold;
unsigned int m_maxIter;
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& timestamp);
virtual void updateKinematics(const Timestamp& timestamp)=0;
virtual void pushCache(const Timestamp& timestamp)=0;
virtual void updateJacobian()=0;
virtual void updateControlOutput(const Timestamp& timestamp)=0;
virtual void initCache(Cache *_cache) = 0;
virtual bool initialise(KDL::Frame& init_pose);
virtual void reset(unsigned int nc,double accuracy,unsigned int maximum_iterations);
virtual bool closeLoop();
virtual double getMaxTimestep(double& timestep);
public:
ConstraintSet(unsigned int nc, double accuracy, unsigned int maximum_iterations);
ConstraintSet();
virtual ~ConstraintSet();
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
ConstraintSet(unsigned int nc,double accuracy,unsigned int maximum_iterations);
ConstraintSet();
virtual ~ConstraintSet();
virtual bool registerCallback(ConstraintCallback _function, void *_param)
{
m_constraintCallback = _function;
m_constraintParam = _param;
return true;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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 bool registerCallback(ConstraintCallback _function, void* _param)
{
m_constraintCallback = _function;
m_constraintParam = _param;
return true;
}
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& 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 &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_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 unsigned int getNrOfConstraints()
{
return m_nc;
};
void substep(bool _substep)
{
m_substep = _substep;
}
bool substep()
{
return m_substep;
}
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;}
};
} // namespace iTaSC
}
#endif /* CONSTRAINTSET_HPP_ */

70
intern/itasc/ControlledObject.cpp
View File

@@ -8,51 +8,55 @@
#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& timestep)
{
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
}

103
intern/itasc/ControlledObject.hpp
View File

@@ -9,92 +9,63 @@
#ifndef CONTROLLEDOBJECT_HPP_
#define CONTROLLEDOBJECT_HPP_
#include "eigen_types.hpp"
#include "kdl/frames.hpp"
#include "eigen_types.hpp"
#include "ConstraintSet.hpp"
#include "Object.hpp"
#include "ConstraintSet.hpp"
#include <vector>
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<e_matrix> m_JqArray;
protected:
e_scalar m_maxDeltaQ;
unsigned int m_nq,m_nc,m_nee;
e_matrix m_Wq,m_Cq;
e_vector m_Wy,m_ydot,m_qdot;
std::vector<e_matrix> m_JqArray;
public:
ControlledObject();
virtual ~ControlledObject();
public:
ControlledObject();
virtual ~ControlledObject();
class JointLockCallback {
public:
JointLockCallback() {}
virtual ~JointLockCallback() {}
class JointLockCallback {
public:
JointLockCallback() {}
virtual ~JointLockCallback() {}
// lock a joint, no need to update output
virtual void lockJoint(unsigned int q_nr, unsigned int ndof) = 0;
// lock a joint and update output in view of reiteration
virtual void lockJoint(unsigned int q_nr, unsigned int ndof, double* qdot) = 0;
};
// lock a joint, no need to update output
virtual void lockJoint(unsigned int q_nr, unsigned int ndof) = 0;
// lock a joint and update output in view of reiteration
virtual void lockJoint(unsigned int q_nr, unsigned int ndof, double *qdot) = 0;
};
virtual void initialize(unsigned int _nq,unsigned int _nc, unsigned int _nee);
virtual void initialize(unsigned int _nq, unsigned int _nc, unsigned int _nee);
// returns true when a joint has been locked via the callback and the solver must run again
virtual bool updateJoint(const Timestamp& timestamp, JointLockCallback& callback) = 0;
virtual void updateControlOutput(const Timestamp& timestamp)=0;
virtual void setJointVelocity(const e_vector qdot_in){m_qdot = qdot_in;};
virtual double getMaxTimestep(double& timestep);
virtual bool setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, e_scalar value, double timestep=0.0)=0;
// returns true when a joint has been locked via the callback and the solver must run again
virtual bool updateJoint(const Timestamp &timestamp, JointLockCallback &callback) = 0;
virtual void updateControlOutput(const Timestamp &timestamp) = 0;
virtual void setJointVelocity(const e_vector qdot_in)
{
m_qdot = qdot_in;
};
virtual double getMaxTimestep(double &timestep);
virtual bool setControlParameter(unsigned int constraintId,
unsigned int valueId,
ConstraintAction action,
e_scalar value,
double timestep = 0.0) = 0;
virtual const e_vector& getControlOutput() const{return m_ydot;}
virtual const e_vector &getControlOutput() const
{
return m_ydot;
}
virtual const e_matrix& getJq(unsigned int ee) const;
virtual const e_matrix &getJq(unsigned int ee) const;
virtual const e_matrix& getCq() const{return m_Cq;};
virtual const e_matrix &getCq() const
{
return m_Cq;
};
virtual e_matrix& getWq() {return m_Wq;};
virtual void setWq(const e_matrix& Wq_in){m_Wq = Wq_in;};
virtual e_matrix &getWq()
{
return m_Wq;
};
virtual void setWq(const e_matrix &Wq_in)
{
m_Wq = Wq_in;
};
virtual const e_vector& getWy() const {return m_Wy;};
virtual const e_vector &getWy() const
{
return m_Wy;
};
virtual const unsigned int getNrOfCoordinates()
{
return m_nq;
};
virtual const unsigned int getNrOfConstraints()
{
return m_nc;
};
virtual const unsigned int getNrOfCoordinates(){return m_nq;};
virtual const unsigned int getNrOfConstraints(){return m_nc;};
};
} // namespace iTaSC
}
#endif /* CONTROLLEDOBJECT_HPP_ */

819
intern/itasc/CopyPose.cpp
View File

@@ -11,500 +11,471 @@
#include <math.h>
#include <string.h>
namespace iTaSC {
const unsigned int maxPoseCacheSize = (2 * (3 + 3 * 2));
CopyPose::CopyPose(unsigned int control_output,
unsigned int dynamic_output,
double armlength,
double accuracy,
unsigned int maximum_iterations)
: ConstraintSet(), m_cache(NULL), m_poseCCh(-1), m_poseCTs(0)
namespace iTaSC
{
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);
const unsigned int maxPoseCacheSize = (2*(3+3*2));
CopyPose::CopyPose(unsigned int control_output, unsigned int dynamic_output, double armlength, double accuracy, unsigned int maximum_iterations):
ConstraintSet(),
m_cache(NULL),
m_poseCCh(-1),m_poseCTs(0)
{
m_maxerror = armlength/2.0;
m_outputControl = (control_output & CTL_ALL);
unsigned int _nc = nBitsOn(m_outputControl);
if (!_nc)
return;
// reset the constraint set
reset(_nc, accuracy, maximum_iterations);
_nc = 0;
m_nvalues = 0;
int nrot = 0, npos = 0;
int nposCache = 0, nrotCache = 0;
m_outputDynamic = (dynamic_output & m_outputControl);
memset(m_values, 0, sizeof(m_values));
memset(m_posData, 0, sizeof(m_posData));
memset(m_rotData, 0, sizeof(m_rotData));
memset(&m_rot, 0, sizeof(m_rot));
memset(&m_pos, 0, sizeof(m_pos));
if (m_outputControl & CTL_POSITION) {
m_pos.alpha = 1.0;
m_pos.K = 20.0;
m_pos.tolerance = 0.05;
m_values[m_nvalues].alpha = m_pos.alpha;
m_values[m_nvalues].feedback = m_pos.K;
m_values[m_nvalues].tolerance = m_pos.tolerance;
m_values[m_nvalues].id = ID_POSITION;
if (m_outputControl & CTL_POSITIONX) {
m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
m_Cf(_nc++,0)=1.0;
m_posData[npos++].id = ID_POSITIONX;
if (m_outputDynamic & CTL_POSITIONX)
nposCache++;
}
if (m_outputControl & CTL_POSITIONY) {
m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
m_Cf(_nc++,1)=1.0;
m_posData[npos++].id = ID_POSITIONY;
if (m_outputDynamic & CTL_POSITIONY)
nposCache++;
}
if (m_outputControl & CTL_POSITIONZ) {
m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
m_Cf(_nc++,2)=1.0;
m_posData[npos++].id = ID_POSITIONZ;
if (m_outputDynamic & CTL_POSITIONZ)
nposCache++;
}
m_values[m_nvalues].number = npos;
m_values[m_nvalues++].values = m_posData;
m_pos.firsty = 0;
m_pos.ny = npos;
}
if (m_outputControl & CTL_ROTATION) {
m_rot.alpha = 1.0;
m_rot.K = 20.0;
m_rot.tolerance = 0.05;
m_values[m_nvalues].alpha = m_rot.alpha;
m_values[m_nvalues].feedback = m_rot.K;
m_values[m_nvalues].tolerance = m_rot.tolerance;
m_values[m_nvalues].id = ID_ROTATION;
if (m_outputControl & CTL_ROTATIONX) {
m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
m_Cf(_nc++,3)=1.0;
m_rotData[nrot++].id = ID_ROTATIONX;
if (m_outputDynamic & CTL_ROTATIONX)
nrotCache++;
}
if (m_outputControl & CTL_ROTATIONY) {
m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
m_Cf(_nc++,4)=1.0;
m_rotData[nrot++].id = ID_ROTATIONY;
if (m_outputDynamic & CTL_ROTATIONY)
nrotCache++;
}
if (m_outputControl & CTL_ROTATIONZ) {
m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
m_Cf(_nc++,5)=1.0;
m_rotData[nrot++].id = ID_ROTATIONZ;
if (m_outputDynamic & CTL_ROTATIONZ)
nrotCache++;
}
m_values[m_nvalues].number = nrot;
m_values[m_nvalues++].values = m_rotData;
m_rot.firsty = npos;
m_rot.ny = nrot;
}
assert(_nc == m_nc);
m_Jf=e_identity_matrix(6,6);
m_poseCacheSize = ((nrotCache)?(3+nrotCache*2):0)+((nposCache)?(3+nposCache*2):0);
}
CopyPose::~CopyPose() {}
bool CopyPose::initialise(Frame &init_pose)
CopyPose::~CopyPose()
{
m_externalPose = m_internalPose = init_pose;
updateJacobian();
return true;
}
void CopyPose::modelUpdate(Frame &_external_pose, const Timestamp &timestamp)
bool CopyPose::initialise(Frame& init_pose)
{
m_internalPose = m_externalPose = _external_pose;
updateJacobian();
m_externalPose = m_internalPose = init_pose;
updateJacobian();
return true;
}
void CopyPose::modelUpdate(Frame& _external_pose,const Timestamp& timestamp)
{
m_internalPose = m_externalPose = _external_pose;
updateJacobian();
}
void CopyPose::initCache(Cache *_cache)
{
m_cache = _cache;
m_poseCCh = -1;
if (m_cache) {
// create one channel for the coordinates
m_poseCCh = m_cache->addChannel(this, "Xf", m_poseCacheSize * sizeof(double));
// don't save initial value, it will be recomputed from external pose
// pushPose(0);
}
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);
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;
}
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;
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;
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, &timestamp);
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++;
bool found = false;
if (m_poseCCh >= 0) {
double *item = (double*)m_cache->getPreviousCacheItem(this, m_poseCCh, &timestamp);
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;
}
}
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& timestamp)
{
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& timestamp)
{
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& 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);
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_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_ALPHA) && _values->alpha >= 0.0) {
_state->alpha = _values->alpha;
action |= ACT_ALPHA;
}
}
if ((_values->action & ACT_TOLERANCE) && _values->tolerance > KDL::epsilon) {
_state->tolerance = _values->tolerance;
action |= ACT_TOLERANCE;
}
if ((_values->action & ACT_FEEDBACK) && _values->feedback > KDL::epsilon) {
_state->K = _values->feedback;
action |= ACT_FEEDBACK;
}
for (i=_state->firsty, j=_state->firsty+_state->ny, _yval=_state->output; i<j; mask <<= 1, id++) {
if (m_outputControl & mask) {
if (action)
m_Wy(i) = _state->alpha/*/(_state->tolerance*_state->K)*/;
// check if this controlled output is provided
for (k=0, _data=_values->values; k<_values->number; k++, _data++) {
if (_data->id == id) {
switch (_data->action & (ACT_VALUE|ACT_VELOCITY)) {
case 0:
// no indication, keep current values
break;
case ACT_VELOCITY:
// only the velocity is given estimate the new value by integration
_data->yd = _yval->yd+_data->yddot*timestep;
// walkthrough
case ACT_VALUE:
_yval->nextyd = _data->yd;
// if the user sets the value, we assume future velocity is zero
// (until the user changes the value again)
_yval->nextyddot = (_data->action & ACT_VALUE) ? 0.0 : _data->yddot;
if (timestep>0.0) {
_yval->yddot = (_data->yd-_yval->yd)/timestep;
} else {
// allow the user to change target instantenously when this function
// if called from setControlParameter with timestep = 0
_yval->yd = _yval->nextyd;
_yval->yddot = _yval->nextyddot;
}
break;
case (ACT_VALUE|ACT_VELOCITY):
// the user should not set the value and velocity at the same time.
// In this case, we will assume that he wants to set the future value
// and we compute the current value to match the velocity
_yval->yd = _data->yd - _data->yddot*timestep;
_yval->nextyd = _data->yd;
_yval->nextyddot = _data->yddot;
if (timestep>0.0) {
_yval->yddot = (_data->yd-_yval->yd)/timestep;
} else {
_yval->yd = _yval->nextyd;
_yval->yddot = _yval->nextyddot;
}
break;
}
}
}
_yval++;
i++;
}
}
}
bool CopyPose::setControlParameters(struct ConstraintValues *_values,
unsigned int _nvalues,
double timestep)
bool CopyPose::setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep)
{
while (_nvalues > 0) {
if (_values->id >= ID_POSITION && _values->id <= ID_POSITIONZ &&
(m_outputControl & CTL_POSITION))
{
updateState(_values, &m_pos, CTL_POSITIONX, timestep);
}
if (_values->id >= ID_ROTATION && _values->id <= ID_ROTATIONZ &&
(m_outputControl & CTL_ROTATION))
{
updateState(_values, &m_rot, CTL_ROTATIONX, timestep);
}
_values++;
_nvalues--;
}
return true;
while (_nvalues > 0) {
if (_values->id >= ID_POSITION && _values->id <= ID_POSITIONZ && (m_outputControl & CTL_POSITION)) {
updateState(_values, &m_pos, CTL_POSITIONX, timestep);
}
if (_values->id >= ID_ROTATION && _values->id <= ID_ROTATIONZ && (m_outputControl & CTL_ROTATION)) {
updateState(_values, &m_rot, CTL_ROTATIONX, timestep);
}
_values++;
_nvalues--;
}
return true;
}
void CopyPose::updateValues(Vector &vel,
ConstraintValues *_values,
ControlState *_state,
unsigned int mask)
void CopyPose::updateValues(Vector& vel, ConstraintValues* _values, ControlState* _state, unsigned int mask)
{
ConstraintSingleValue *_data;
ControlState::ControlValue *_yval;
int i, j;
ConstraintSingleValue* _data;
ControlState::ControlValue* _yval;
int i, j;
_values->action = 0;
_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++;
}
}
for (i=_state->firsty, j=0, _yval=_state->output, _data=_values->values; j<3; j++, mask<<=1) {
if (m_outputControl & mask) {
*(double*)&_data->y = vel(j);
*(double*)&_data->ydot = m_ydot(i);
_data->yd = _yval->yd;
_data->yddot = _yval->yddot;
_data->action = 0;
i++;
_data++;
_yval++;
}
}
}
void CopyPose::updateOutput(Vector &vel, ControlState *_state, unsigned int mask)
void CopyPose::updateOutput(Vector& vel, ControlState* _state, unsigned int mask)
{
ControlState::ControlValue *_yval;
int i, j;
double coef = 1.0;
if (mask & CTL_POSITION) {
// put a limit on position error
double len = 0.0;
for (j = 0, _yval = _state->output; j < 3; j++) {
if (m_outputControl & (mask << j)) {
len += KDL::sqr(_yval->yd - vel(j));
_yval++;
}
}
len = KDL::sqrt(len);
if (len > m_maxerror)
coef = m_maxerror / len;
}
for (i = _state->firsty, j = 0, _yval = _state->output; j < 3; j++) {
if (m_outputControl & (mask << j)) {
m_ydot(i) = _yval->yddot + _state->K * coef * (_yval->yd - vel(j));
_yval++;
i++;
}
}
ControlState::ControlValue* _yval;
int i, j;
double coef=1.0;
if (mask & CTL_POSITION) {
// put a limit on position error
double len=0.0;
for (j=0, _yval=_state->output; j<3; j++) {
if (m_outputControl & (mask<<j)) {
len += KDL::sqr(_yval->yd-vel(j));
_yval++;
}
}
len = KDL::sqrt(len);
if (len > m_maxerror)
coef = m_maxerror/len;
}
for (i=_state->firsty, j=0, _yval=_state->output; j<3; j++) {
if (m_outputControl & (mask<<j)) {
m_ydot(i)=_yval->yddot+_state->K*coef*(_yval->yd-vel(j));
_yval++;
i++;
}
}
}
void CopyPose::updateControlOutput(const Timestamp &timestamp)
void CopyPose::updateControlOutput(const Timestamp& timestamp)
{
// IMO this should be done, no idea if it is enough (wrt Distance impl)
Twist y = diff(F_identity, m_internalPose);
bool found = true;
if (!timestamp.substep) {
if (!timestamp.reiterate) {
found = popPose(timestamp.cacheTimestamp);
}
}
if (m_constraintCallback && (m_substep || (!timestamp.reiterate && !timestamp.substep))) {
// initialize first callback the application to get the current values
int i = 0;
if (m_outputControl & CTL_POSITION) {
updateValues(y.vel, &m_values[i++], &m_pos, CTL_POSITIONX);
}
if (m_outputControl & CTL_ROTATION) {
updateValues(y.rot, &m_values[i++], &m_rot, CTL_ROTATIONX);
}
if ((*m_constraintCallback)(timestamp, m_values, m_nvalues, m_constraintParam)) {
setControlParameters(
m_values, m_nvalues, (found && timestamp.interpolate) ? timestamp.realTimestep : 0.0);
}
}
if (m_outputControl & CTL_POSITION) {
updateOutput(y.vel, &m_pos, CTL_POSITIONX);
}
if (m_outputControl & CTL_ROTATION) {
updateOutput(y.rot, &m_rot, CTL_ROTATIONX);
}
//IMO this should be done, no idea if it is enough (wrt Distance impl)
Twist y = diff(F_identity, m_internalPose);
bool found = true;
if (!timestamp.substep) {
if (!timestamp.reiterate) {
found = popPose(timestamp.cacheTimestamp);
}
}
if (m_constraintCallback && (m_substep || (!timestamp.reiterate && !timestamp.substep))) {
// initialize first callback the application to get the current values
int i=0;
if (m_outputControl & CTL_POSITION) {
updateValues(y.vel, &m_values[i++], &m_pos, CTL_POSITIONX);
}
if (m_outputControl & CTL_ROTATION) {
updateValues(y.rot, &m_values[i++], &m_rot, CTL_ROTATIONX);
}
if ((*m_constraintCallback)(timestamp, m_values, m_nvalues, m_constraintParam)) {
setControlParameters(m_values, m_nvalues, (found && timestamp.interpolate)?timestamp.realTimestep:0.0);
}
}
if (m_outputControl & CTL_POSITION) {
updateOutput(y.vel, &m_pos, CTL_POSITIONX);
}
if (m_outputControl & CTL_ROTATION) {
updateOutput(y.rot, &m_rot, CTL_ROTATIONX);
}
}
const ConstraintValues *CopyPose::getControlParameters(unsigned int *_nvalues)
const ConstraintValues* CopyPose::getControlParameters(unsigned int* _nvalues)
{
Twist y = diff(m_internalPose, F_identity);
int i = 0;
if (m_outputControl & CTL_POSITION) {
updateValues(y.vel, &m_values[i++], &m_pos, CTL_POSITIONX);
}
if (m_outputControl & CTL_ROTATION) {
updateValues(y.rot, &m_values[i++], &m_rot, CTL_ROTATIONX);
}
if (_nvalues)
*_nvalues = m_nvalues;
return m_values;
Twist y = diff(m_internalPose,F_identity);
int i=0;
if (m_outputControl & CTL_POSITION) {
updateValues(y.vel, &m_values[i++], &m_pos, CTL_POSITIONX);
}
if (m_outputControl & CTL_ROTATION) {
updateValues(y.rot, &m_values[i++], &m_rot, CTL_ROTATIONX);
}
if (_nvalues)
*_nvalues=m_nvalues;
return m_values;
}
double CopyPose::getMaxTimestep(double &timestep)
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;
// 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
}

177
intern/itasc/CopyPose.hpp
View File

@@ -10,114 +10,91 @@
#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& timestamp);
virtual void pushCache(const Timestamp& timestamp);
virtual void updateJacobian();
virtual bool initialise(Frame& init_pose);
virtual void initCache(Cache *_cache);
virtual void updateControlOutput(const Timestamp& timestamp);
virtual void modelUpdate(Frame& _external_pose,const Timestamp& timestamp);
virtual double getMaxTimestep(double& timestep);
public:
enum ID { // constraint ID in callback and setControlParameter
ID_POSITION = 0,
ID_POSITIONX = 1,
ID_POSITIONY = 2,
ID_POSITIONZ = 3,
ID_ROTATION = 4,
ID_ROTATIONX = 5,
ID_ROTATIONY = 6,
ID_ROTATIONZ = 7,
};
enum CTL { // control ID in constructor to specify which output is constrainted
CTL_NONE = 0x00,
CTL_POSITIONX = 0x01, // the bit order is important: it matches the y output order
CTL_POSITIONY = 0x02,
CTL_POSITIONZ = 0x04,
CTL_POSITION = 0x07,
CTL_ROTATIONX = 0x08,
CTL_ROTATIONY = 0x10,
CTL_ROTATIONZ = 0x20,
CTL_ROTATION = 0x38,
CTL_ALL = 0x3F,
};
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;
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);
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);
};
} // namespace iTaSC
}
#endif /* COPYROTATION_H_ */

526
intern/itasc/Distance.cpp
View File

@@ -11,322 +11,312 @@
#include <math.h>
#include <string.h>
namespace iTaSC {
// a distance constraint is characterized by 5 values: alpha, tolerance, K, yd, yddot
static const unsigned int distanceCacheSize = sizeof(double) * 5 + sizeof(e_scalar) * 6;
Distance::Distance(double armlength, double accuracy, unsigned int maximum_iterations)
: ConstraintSet(1, accuracy, maximum_iterations),
m_chiKdl(6),
m_jac(6),
m_cache(NULL),
m_distCCh(-1),
m_distCTs(0)
namespace iTaSC
{
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)));
// 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;
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(double armlength, double accuracy, unsigned int maximum_iterations):
ConstraintSet(1,accuracy,maximum_iterations),
m_chiKdl(6),m_jac(6),m_cache(NULL),
m_distCCh(-1),m_distCTs(0)
{
m_chain.addSegment(Segment(Joint(Joint::RotZ)));
m_chain.addSegment(Segment(Joint(Joint::RotX)));
m_chain.addSegment(Segment(Joint(Joint::TransY)));
m_chain.addSegment(Segment(Joint(Joint::RotZ)));
m_chain.addSegment(Segment(Joint(Joint::RotY)));
m_chain.addSegment(Segment(Joint(Joint::RotX)));
m_fksolver = new KDL::ChainFkSolverPos_recursive(m_chain);
m_jacsolver = new KDL::ChainJntToJacSolver(m_chain);
m_Cf(0,2)=1.0;
m_alpha = 1.0;
m_tolerance = 0.05;
m_maxerror = armlength/2.0;
m_K = 20.0;
m_Wy(0) = m_alpha/*/(m_tolerance*m_K)*/;
m_yddot = m_nextyddot = 0.0;
m_yd = m_nextyd = KDL::epsilon;
memset(&m_data, 0, sizeof(m_data));
// initialize the data with normally fixed values
m_data.id = ID_DISTANCE;
m_values.id = ID_DISTANCE;
m_values.number = 1;
m_values.alpha = m_alpha;
m_values.feedback = m_K;
m_values.tolerance = m_tolerance;
m_values.values = &m_data;
}
Distance::~Distance()
{
delete m_fksolver;
delete m_jacsolver;
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, &timestamp);
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, &timestamp);
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& timestamp)
{
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& 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);
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& timestamp)
{
bool cacheAvail = true;
if (!timestamp.substep) {
if (!timestamp.reiterate)
cacheAvail = popDist(timestamp.cacheTimestamp);
}
if (m_constraintCallback && (m_substep || (!timestamp.reiterate && !timestamp.substep))) {
// initialize first callback the application to get the current values
*(double *)&m_data.y = m_chi(2);
*(double *)&m_data.ydot = m_ydot(0);
m_data.yd = m_yd;
m_data.yddot = m_yddot;
m_data.action = 0;
m_values.action = 0;
if ((*m_constraintCallback)(timestamp, &m_values, 1, m_constraintParam)) {
setControlParameters(&m_values, 1, timestamp.realTimestep);
}
}
if (!cacheAvail || !timestamp.interpolate) {
// first position in cache: set the desired output immediately as we cannot interpolate
m_yd = m_nextyd;
m_yddot = m_nextyddot;
}
double error = m_yd - m_chi(2);
if (KDL::Norm(error) > m_maxerror)
error = KDL::sign(error) * m_maxerror;
m_ydot(0) = m_yddot + m_K * error;
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
}

74
intern/itasc/Distance.hpp
View File

@@ -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& timestamp);
virtual void pushCache(const Timestamp& timestamp);
virtual void updateJacobian();
virtual bool initialise(Frame& init_pose);
virtual void initCache(Cache *_cache);
virtual void updateControlOutput(const Timestamp& timestamp);
virtual bool closeLoop();
public:
enum ID {
ID_DISTANCE = 1,
};
Distance(double armlength = 1.0, double accuracy = 1e-6, unsigned int maximum_iterations = 100);
virtual ~Distance();
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_ */

85
intern/itasc/FixedObject.cpp
View File

@@ -8,62 +8,65 @@
#include "FixedObject.hpp"
namespace iTaSC {
namespace iTaSC{
FixedObject::FixedObject() : UncontrolledObject(), m_finalized(false), m_nframe(0) {}
FixedObject::~FixedObject()
FixedObject::FixedObject():UncontrolledObject(),
m_finalized(false), m_nframe(0)
{
m_frameArray.clear();
}
int FixedObject::addFrame(const std::string &name, const Frame &frame)
FixedObject::~FixedObject()
{
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++;
m_frameArray.clear();
}
int FixedObject::addEndEffector(const std::string &name)
int FixedObject::addFrame(const std::string& name, const Frame& frame)
{
// 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;
if (m_finalized)
return -1;
FrameList::iterator it;
unsigned int i;
for (i=0, it=m_frameArray.begin(); i<m_nframe; i++, it++) {
if (it->first == name) {
// this frame will replace the old frame
it->second = frame;
return i;
}
}
m_frameArray.push_back(FrameList::value_type(name,frame));
return m_nframe++;
}
int FixedObject::addEndEffector(const std::string& name)
{
// verify that this frame name exist
FrameList::iterator it;
unsigned int i;
for (i=0, it=m_frameArray.begin(); i<m_nframe; i++, it++) {
if (it->first == name) {
return i;
}
}
return -1;
}
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
}

43
intern/itasc/FixedObject.hpp
View File

@@ -12,34 +12,35 @@
#include "UncontrolledObject.hpp"
#include <vector>
namespace iTaSC {
class FixedObject : public UncontrolledObject {
public:
FixedObject();
virtual ~FixedObject();
namespace iTaSC{
int addFrame(const std::string &name, const Frame &frame);
class FixedObject: public UncontrolledObject {
public:
FixedObject();
virtual ~FixedObject();
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){};
int addFrame(const std::string& name, const Frame& frame);
protected:
virtual void updateJacobian() {}
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) {};
private:
typedef std::vector<std::pair<std::string, Frame>> FrameList;
protected:
virtual void updateJacobian() {}
private:
typedef std::vector<std::pair<std::string, Frame> > FrameList;
bool m_finalized;
unsigned int m_nframe;
FrameList m_frameArray;
bool m_finalized;
unsigned int m_nframe;
FrameList m_frameArray;
};
} // namespace iTaSC
}
#endif /* FIXEDOBJECT_H_ */

216
intern/itasc/MovingFrame.cpp
View File

@@ -8,161 +8,151 @@
#include "MovingFrame.hpp"
#include <string.h>
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, &timestamp);
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, &timestamp);
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& timestamp)
{
// don't compute the velocity during substepping, it is assumed constant.
if (!timestamp.substep) {
bool cacheAvail = true;
if (!timestamp.reiterate) {
cacheAvail = popInternalFrame(timestamp.cacheTimestamp);
if (m_function)
(*m_function)(timestamp, m_internalPose, m_nextPose, m_param);
}
// only compute velocity if we have a previous pose
if (cacheAvail && timestamp.interpolate) {
unsigned int iXu;
m_velocity = diff(m_internalPose, m_nextPose, timestamp.realTimestep);
for (iXu = 0; iXu < 6; iXu++)
m_xudot(iXu) = m_velocity(iXu);
}
else if (!timestamp.reiterate) {
// new position is forced, no velocity as we cannot interpolate
m_internalPose = m_nextPose;
m_velocity = Twist::Zero();
m_xudot = e_zero_vector(6);
// recompute the jacobian
updateJacobian();
}
}
// 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& timestamp)
{
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& 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);
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
}

57
intern/itasc/MovingFrame.hpp
View File

@@ -12,41 +12,42 @@
#include "UncontrolledObject.hpp"
#include <vector>
namespace iTaSC {
typedef bool (*MovingFrameCallback)(const Timestamp &timestamp,
const Frame &_current,
Frame &_next,
void *param);
namespace iTaSC{
class MovingFrame : public UncontrolledObject {
public:
MovingFrame(const Frame &frame = F_identity);
virtual ~MovingFrame();
typedef bool (*MovingFrameCallback)(
const Timestamp& timestamp,
const Frame& _current,
Frame& _next,
void *param);
bool setFrame(const Frame &frame);
bool setCallback(MovingFrameCallback _function, void *_param);
class MovingFrame: public UncontrolledObject {
public:
MovingFrame(const Frame& frame=F_identity);
virtual ~MovingFrame();
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();
bool setFrame(const Frame& frame);
bool setCallback(MovingFrameCallback _function, void* _param);
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);
virtual bool finalize();
protected:
virtual void updateJacobian();
private:
void pushInternalFrame(CacheTS timestamp);
bool popInternalFrame(CacheTS timestamp);
MovingFrameCallback m_function;
void *m_param;
Frame m_nextPose;
Twist m_velocity;
int m_poseCCh; // cache channel for pose
unsigned int m_poseCTs;
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_ */

78
intern/itasc/Object.hpp
View File

@@ -13,62 +13,40 @@
#include "kdl/frames.hpp"
#include <string>
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;
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(){};
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;
};
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_ */

20
intern/itasc/Scene.cpp
View File

@@ -19,8 +19,12 @@ 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)
{
@@ -371,8 +375,7 @@ 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
@@ -388,8 +391,7 @@ 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);
@@ -438,8 +440,7 @@ 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,
@@ -478,8 +479,7 @@ 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);

158
intern/itasc/Scene.hpp
View File

@@ -11,9 +11,9 @@
#include "eigen_types.hpp"
#include "WorldObject.hpp"
#include "ConstraintSet.hpp"
#include "Solver.hpp"
#include "WorldObject.hpp"
#include <map>
@@ -22,110 +22,84 @@ namespace iTaSC {
class SceneLock;
class Scene {
friend class SceneLock;
friend class SceneLock;
public:
enum SceneParam {
MIN_TIMESTEP = 0,
MAX_TIMESTEP,
public:
enum SceneParam {
MIN_TIMESTEP = 0,
MAX_TIMESTEP,
COUNT
};
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<bool> 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<bool> 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
Object_struct(Object *_object,
UncontrolledObject *_base,
unsigned int _baseFrameIndex,
Range nq_range,
Range nc_range,
Range nu_range)
: object(_object),
base(_base),
baseFrameIndex(_baseFrameIndex),
constraintrange(nc_range),
jointrange(nq_range),
coordinaterange(nu_range){};
};
typedef std::map<std::string, Object_struct *> ObjectMap;
struct 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 ConstraintSet_struct {
ConstraintSet *task;
ObjectMap::iterator object1;
ObjectMap::iterator object2;
Range constraintrange;
Range featurerange;
unsigned int ee1index;
unsigned int ee2index;
ConstraintSet_struct(ConstraintSet *_task,
ObjectMap::iterator _object1,
unsigned int _ee1index,
ObjectMap::iterator _object2,
unsigned int _ee2index,
Range nc_range,
Range coord_range)
: task(_task),
object1(_object1),
object2(_object2),
constraintrange(nc_range),
featurerange(coord_range),
ee1index(_ee1index),
ee2index(_ee2index){};
};
typedef std::map<std::string, ConstraintSet_struct *> ConstraintMap;
Object_struct(Object* _object,UncontrolledObject* _base,unsigned int _baseFrameIndex,Range nq_range,Range nc_range,Range nu_range):
object(_object),base(_base),baseFrameIndex(_baseFrameIndex),constraintrange(nc_range),jointrange(nq_range),coordinaterange(nu_range)
{};
};
typedef std::map<std::string,Object_struct*> ObjectMap;
ObjectMap objects;
ConstraintMap constraints;
struct ConstraintSet_struct{
ConstraintSet* task;
ObjectMap::iterator object1;
ObjectMap::iterator object2;
Range constraintrange;
Range featurerange;
unsigned int ee1index;
unsigned int ee2index;
ConstraintSet_struct(ConstraintSet* _task,
ObjectMap::iterator _object1,unsigned int _ee1index,
ObjectMap::iterator _object2,unsigned int _ee2index,
Range nc_range,Range coord_range):
task(_task),
object1(_object1),object2(_object2),
constraintrange(nc_range),featurerange(coord_range),
ee1index(_ee1index), ee2index(_ee2index)
{};
};
typedef std::map<std::string,ConstraintSet_struct*> ConstraintMap;
static bool getConstraintPose(ConstraintSet *constraint, void *_param, KDL::Frame &_pose);
ObjectMap objects;
ConstraintMap constraints;
static bool getConstraintPose(ConstraintSet* constraint, void *_param, KDL::Frame& _pose);
};
} // namespace iTaSC
}
#endif /* SCENE_HPP_ */

33
intern/itasc/Solver.hpp
View File

@@ -9,27 +9,26 @@
#ifndef SOLVER_HPP_
#define SOLVER_HPP_
#include "eigen_types.hpp"
#include <vector>
#include "eigen_types.hpp"
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<bool> &gc) = 0;
virtual bool solve(const e_matrix &A,
const e_vector &Wy,
const e_vector &ydot,
const e_matrix &Wq,
e_vector &qdot,
e_scalar &nlcoef) = 0;
virtual void setParam(SolverParam param, double value) = 0;
// gc = grouping of constraint output ,
// size of vector = nc, alternance of true / false to indicate the grouping of output
virtual bool init(unsigned int nq, unsigned int nc, const std::vector<bool>& gc)=0;
virtual bool solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef)=0;
virtual void setParam(SolverParam param, double value)=0;
};
} // namespace iTaSC
}
#endif /* SOLVER_HPP_ */

41
intern/itasc/UncontrolledObject.cpp
View File

@@ -8,30 +8,37 @@
#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
}

42
intern/itasc/UncontrolledObject.hpp
View File

@@ -12,35 +12,27 @@
#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<e_matrix> m_JuArray;
class UncontrolledObject: public Object {
protected:
unsigned int m_nu, m_nf;
e_vector m_xudot;
std::vector<e_matrix> m_JuArray;
public:
UncontrolledObject();
virtual ~UncontrolledObject();
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;};
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;
};
};
} // namespace iTaSC
}
#endif /* UNCONTROLLEDOBJECT_H_ */

170
intern/itasc/WDLSSolver.cpp
View File

@@ -11,100 +11,92 @@
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() {}
bool WDLSSolver::init(unsigned int nq, unsigned int nc, const std::vector<bool> &gc)
{
m_ns = std::min(nc, nq);
m_AWq = e_zero_matrix(nc, nq);
m_WyAWq = e_zero_matrix(nc, nq);
m_WyAWqt = e_zero_matrix(nq, nc);
m_S = e_zero_vector(std::max(nc, nq));
m_Wy_ydot = e_zero_vector(nc);
if (nq > nc) {
m_transpose = true;
m_temp = e_zero_vector(nc);
m_U = e_zero_matrix(nc, nc);
m_V = e_zero_matrix(nq, nc);
m_WqV = e_zero_matrix(nq, nc);
}
else {
m_transpose = false;
m_temp = e_zero_vector(nq);
m_U = e_zero_matrix(nc, nq);
m_V = e_zero_matrix(nq, nq);
m_WqV = e_zero_matrix(nq, nq);
}
return true;
WDLSSolver::~WDLSSolver() {
}
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::init(unsigned int nq, unsigned int nc, const std::vector<bool>& gc)
{
double alpha, vmax, norm;
// Create the Weighted jacobian
m_AWq = A * Wq;
for (int i = 0; i < Wy.size(); i++)
m_WyAWq.row(i) = Wy(i) * m_AWq.row(i);
// Compute the SVD of the weighted jacobian
int ret;
if (m_transpose) {
m_WyAWqt = m_WyAWq.transpose();
ret = KDL::svd_eigen_HH(m_WyAWqt, m_V, m_S, m_U, m_temp);
}
else {
ret = KDL::svd_eigen_HH(m_WyAWq, m_U, m_S, m_V, m_temp);
}
if (ret < 0)
return false;
m_WqV.noalias() = Wq * m_V;
// 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;
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;
}
} // namespace iTaSC
bool WDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef)
{
double alpha, vmax, norm;
// Create the Weighted jacobian
m_AWq = A*Wq;
for (int i=0; i<Wy.size(); i++)
m_WyAWq.row(i) = Wy(i)*m_AWq.row(i);
// Compute the SVD of the weighted jacobian
int ret;
if (m_transpose) {
m_WyAWqt = m_WyAWq.transpose();
ret = KDL::svd_eigen_HH(m_WyAWqt,m_V,m_S,m_U,m_temp);
} else {
ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp);
}
if(ret<0)
return false;
m_WqV.noalias() = Wq*m_V;
//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;
}
}

64
intern/itasc/WDLSSolver.hpp
View File

@@ -13,43 +13,37 @@
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;
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();
public:
WDLSSolver();
virtual ~WDLSSolver();
virtual bool init(unsigned int nq, unsigned int nc, const std::vector<bool> &gc);
virtual bool solve(const e_matrix &A,
const e_vector &Wy,
const e_vector &ydot,
const e_matrix &Wq,
e_vector &qdot,
e_scalar &nlcoef);
virtual void setParam(SolverParam param, double value)
{
switch (param) {
case DLS_QMAX:
m_qmax = value;
break;
case DLS_LAMBDA_MAX:
m_lambda = value;
break;
case DLS_EPSILON:
m_epsilon = value;
break;
}
}
virtual bool init(unsigned int nq, unsigned int nc, const std::vector<bool>& gc);
virtual bool solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef);
virtual void setParam(SolverParam param, double value)
{
switch (param) {
case DLS_QMAX:
m_qmax = value;
break;
case DLS_LAMBDA_MAX:
m_lambda = value;
break;
case DLS_EPSILON:
m_epsilon = value;
break;
}
}
};
} // namespace iTaSC
}
#endif /* WDLSSOLVER_HPP_ */

230
intern/itasc/WSDLSSolver.cpp
View File

@@ -12,136 +12,128 @@
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<bool> &gc)
bool WSDLSSolver::init(unsigned int _nq, unsigned int _nc, const std::vector<bool>& gc)
{
if (_nc == 0 || _nq == 0 || gc.size() != _nc)
return false;
m_nc = _nc;
m_nq = _nq;
m_ns = std::min(m_nc, m_nq);
m_AWq = e_zero_matrix(m_nc, m_nq);
m_WyAWq = e_zero_matrix(m_nc, m_nq);
m_WyAWqt = e_zero_matrix(m_nq, m_nc);
m_S = e_zero_vector(std::max(m_nc, m_nq));
m_Wy_ydot = e_zero_vector(m_nc);
m_ytask = gc;
if (m_nq > m_nc) {
m_transpose = true;
m_temp = e_zero_vector(m_nc);
m_U = e_zero_matrix(m_nc, m_nc);
m_V = e_zero_matrix(m_nq, m_nc);
m_WqV = e_zero_matrix(m_nq, m_nc);
}
else {
m_transpose = false;
m_temp = e_zero_vector(m_nq);
m_U = e_zero_matrix(m_nc, m_nq);
m_V = e_zero_matrix(m_nq, m_nq);
m_WqV = e_zero_matrix(m_nq, m_nq);
}
return true;
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 < m_nc; i++)
m_WyAWq.row(i) = Wy(i) * m_AWq.row(i);
// Create the Weighted jacobian
m_AWq.noalias() = A*Wq;
for (i=0; i<m_nc; i++)
m_WyAWq.row(i) = Wy(i)*m_AWq.row(i);
// Compute the SVD of the weighted jacobian
int ret;
if (m_transpose) {
m_WyAWqt = m_WyAWq.transpose();
ret = KDL::svd_eigen_HH(m_WyAWqt, m_V, m_S, m_U, m_temp);
}
else {
ret = KDL::svd_eigen_HH(m_WyAWq, m_U, m_S, m_V, m_temp);
}
if (ret < 0)
return false;
// Compute the SVD of the weighted jacobian
int ret;
if (m_transpose) {
m_WyAWqt = m_WyAWq.transpose();
ret = KDL::svd_eigen_HH(m_WyAWqt,m_V,m_S,m_U,m_temp);
} else {
ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp);
}
if(ret<0)
return false;
m_Wy_ydot = Wy.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;
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
}

54
intern/itasc/WSDLSSolver.hpp
View File

@@ -13,38 +13,32 @@
namespace iTaSC {
class WSDLSSolver : public iTaSC::Solver {
private:
e_matrix m_AWq, m_WyAWq, m_WyAWqt, m_U, m_V, m_WqV;
e_vector m_S, m_temp, m_Wy_ydot;
std::vector<bool> m_ytask;
e_scalar m_qmax;
unsigned int m_ns, m_nc, m_nq;
bool m_transpose;
class WSDLSSolver: public iTaSC::Solver {
private:
e_matrix m_AWq,m_WyAWq,m_WyAWqt,m_U,m_V,m_WqV;
e_vector m_S,m_temp,m_Wy_ydot;
std::vector<bool> m_ytask;
e_scalar m_qmax;
unsigned int m_ns, m_nc, m_nq;
bool m_transpose;
public:
WSDLSSolver();
virtual ~WSDLSSolver();
public:
WSDLSSolver();
virtual ~WSDLSSolver();
virtual bool init(unsigned int _nq, unsigned int _nc, const std::vector<bool> &gc);
virtual bool solve(const e_matrix &A,
const e_vector &Wy,
const e_vector &ydot,
const e_matrix &Wq,
e_vector &qdot,
e_scalar &nlcoef);
virtual void setParam(SolverParam param, double value)
{
switch (param) {
case DLS_QMAX:
m_qmax = value;
break;
default:
break;
}
}
virtual bool init(unsigned int _nq, unsigned int _nc, const std::vector<bool>& gc);
virtual bool solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef);
virtual void setParam(SolverParam param, double value)
{
switch (param) {
case DLS_QMAX:
m_qmax = value;
break;
default:
break;
}
}
};
} // namespace iTaSC
}
#endif /* WSDLSSOLVER_HPP_ */

15
intern/itasc/WorldObject.cpp
View File

@@ -8,17 +8,20 @@
#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
}

24
intern/itasc/WorldObject.hpp
View File

@@ -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*/){};
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() {}
protected:
virtual void updateJacobian() {}
};
} // namespace iTaSC
}
#endif /* WORLDOBJECT_H_ */

4
intern/itasc/eigen_types.cpp
View File

@@ -8,4 +8,6 @@
#include "eigen_types.hpp"
const KDL::Frame iTaSC::F_identity(Rotation::Identity(), Vector::Zero());
const KDL::Frame iTaSC::F_identity(Rotation::Identity(),Vector::Zero());

86
intern/itasc/eigen_types.hpp
View File

@@ -9,28 +9,29 @@
#ifndef EIGEN_TYPES_HPP_
#define EIGEN_TYPES_HPP_
#include "kdl/chain.hpp"
#include <Eigen/Core>
#include "kdl/frames.hpp"
#include "kdl/tree.hpp"
#include "kdl/chain.hpp"
#include "kdl/jacobian.hpp"
#include "kdl/jntarray.hpp"
#include "kdl/tree.hpp"
#include <Eigen/Core>
namespace iTaSC {
using KDL::Chain;
namespace iTaSC{
using KDL::Twist;
using KDL::Frame;
using KDL::Inertia;
using KDL::Jacobian;
using KDL::JntArray;
using KDL::Joint;
using KDL::Rotation;
using KDL::Segment;
using KDL::Inertia;
using KDL::SegmentMap;
using KDL::Tree;
using KDL::Twist;
using KDL::JntArray;
using KDL::Jacobian;
using KDL::Segment;
using KDL::Rotation;
using KDL::Vector;
using KDL::Vector2;
using KDL::Chain;
extern const Frame F_identity;
@@ -38,58 +39,47 @@ extern const Frame F_identity;
#define e_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>
#define e_zero_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>::Zero
#define e_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>
#define e_matrix6 Eigen::Matrix<e_scalar, 6, 6>
#define e_matrix6 Eigen::Matrix<e_scalar,6,6>
#define e_identity_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Identity
#define e_scalar_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>::Constant
#define e_zero_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero
#define e_random_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Random
#define e_vector6 Eigen::Matrix<e_scalar, 6, 1>
#define e_vector3 Eigen::Matrix<e_scalar, 3, 1>
#define e_vector6 Eigen::Matrix<e_scalar,6,1>
#define e_vector3 Eigen::Matrix<e_scalar,3,1>
class Range {
public:
int start;
int count;
Range(int _start, int _count)
{
start = _start;
count = _count;
}
Range(const Range &other)
{
start = other.start;
count = other.count;
}
public:
int start;
int count;
Range(int _start, int _count) { start = _start; count=_count; }
Range(const Range& other) { start=other.start; count=other.count; }
};
template<typename MatrixType> inline Eigen::Block<MatrixType> project(MatrixType &m, Range r)
template<typename MatrixType> inline Eigen::Block<MatrixType> project(MatrixType& m, Range r)
{
return Eigen::Block<MatrixType>(m, r.start, 0, r.count, 1);
return Eigen::Block<MatrixType>(m,r.start,0,r.count,1);
}
template<typename MatrixType>
inline Eigen::Block<MatrixType> project(MatrixType &m, Range r, Range c)
template<typename MatrixType> inline Eigen::Block<MatrixType> project(MatrixType& m, Range r, Range c)
{
return Eigen::Block<MatrixType>(m, r.start, c.start, r.count, c.count);
return Eigen::Block<MatrixType>(m,r.start,c.start,r.count,c.count);
}
template<typename Derived>
inline static int changeBase(Eigen::MatrixBase<Derived> &J, const Frame &T)
{
template<typename Derived> inline static int changeBase(Eigen::MatrixBase<Derived>& J, const Frame& T) {
if (J.rows() != 6)
return -1;
for (int j = 0; j < J.cols(); ++j) {
typename Derived::ColXpr Jj = J.col(j);
Twist arg;
for (unsigned int i = 0; i < 6; ++i)
arg(i) = Jj[i];
Twist tmp(T * arg);
for (unsigned int i = 0; i < 6; ++i)
Jj[i] = e_scalar(tmp(i));
}
return 0;
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_ */

84
intern/itasc/kdl/chain.cpp
View File

@@ -25,44 +25,54 @@
#include "chain.hpp"
namespace KDL {
using namespace std;
using namespace std;
Chain::Chain() : segments(0), nrOfJoints(0), nrOfSegments(0) {}
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()
{
}
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

127
intern/itasc/kdl/chain.hpp
View File

@@ -23,80 +23,75 @@
#define KDL_CHAIN_HPP
#include "segment.hpp"
#include <Eigen/StdVector>
#include <string>
#include <Eigen/StdVector>
namespace KDL {
/**
* \brief This class encapsulates a <strong>serial</strong> 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<Segment, Eigen::aligned_allocator<Segment>> segments;
unsigned int nrOfJoints;
unsigned int nrOfSegments;
/**
* \brief This class encapsulates a <strong>serial</strong> 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<Segment, Eigen::aligned_allocator<Segment> > segments;
unsigned int nrOfJoints;
unsigned int nrOfSegments;
public:
/**
* The constructor of a chain, a new chain is always empty.
*
*/
Chain();
Chain(const Chain& in);
Chain& operator = (const Chain& arg);
public:
/**
* The constructor of a chain, a new chain is always empty.
*
*/
Chain();
Chain(const Chain &in);
Chain &operator=(const Chain &arg);
/**
* Adds a new segment to the <strong>end</strong> of the chain.
*
* @param segment The segment to add
*/
void addSegment(const Segment& segment);
/**
* Adds a complete chain to the <strong>end</strong> of the chain
* The added chain is copied.
*
* @param chain The chain to add
*/
void addChain(const Chain& chain);
/**
* Adds a new segment to the <strong>end</strong> of the chain.
*
* @param segment The segment to add
*/
void addSegment(const Segment &segment);
/**
* Adds a complete chain to the <strong>end</strong> of the chain
* The added chain is copied.
*
* @param chain The chain to add
*/
void addChain(const Chain &chain);
/**
* Request the total number of joints in the chain.\n
* <strong> Important:</strong> It is not the
* same as the total number of segments since a segment does not
* need to have a joint. This function is important when
* creating a KDL::JntArray to use with this chain.
* @return total nr of joints
*/
unsigned int getNrOfJoints()const {return nrOfJoints;};
/**
* Request the total number of segments in the chain.
* @return total number of segments
*/
unsigned int getNrOfSegments()const {return nrOfSegments;};
/**
* Request the total number of joints in the chain.\n
* <strong> Important:</strong> It is not the
* same as the total number of segments since a segment does not
* need to have a joint. This function is important when
* creating a KDL::JntArray to use with this chain.
* @return total nr of joints
*/
unsigned int getNrOfJoints() const
{
return nrOfJoints;
};
/**
* Request the total number of segments in the chain.
* @return total number of segments
*/
unsigned int getNrOfSegments() const
{
return nrOfSegments;
};
/**
* Request the nr'd segment of the chain. There is no boundary
* checking.
*
* @param nr the nr of the segment starting from 0
*
* @return a constant reference to the nr'd segment
*/
const Segment& getSegment(unsigned int nr)const;
/**
* 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();
};
virtual ~Chain();
};
} // end of namespace KDL
}//end of namespace KDL
#endif

133
intern/itasc/kdl/chainfksolver.hpp
View File

@@ -23,84 +23,85 @@
#define KDL_CHAIN_FKSOLVER_HPP
#include "chain.hpp"
#include "frameacc.hpp"
#include "framevel.hpp"
#include "frameacc.hpp"
#include "jntarray.hpp"
#include "jntarrayacc.hpp"
#include "jntarrayvel.hpp"
#include "jntarrayacc.hpp"
namespace KDL {
/**
* \brief This <strong>abstract</strong> class encapsulates a
* solver for the forward position kinematics for a KDL::Chain.
*
* @ingroup KinematicFamily
*/
/**
* \brief This <strong>abstract</strong> class encapsulates a
* solver for the forward position kinematics for a KDL::Chain.
*
* @ingroup KinematicFamily
*/
// Forward definition
class ChainFkSolverPos {
public:
/**
* Calculate forward position kinematics for a KDL::Chain,
* from joint coordinates to cartesian pose.
*
* @param q_in input joint coordinates
* @param p_out reference to output cartesian pose
*
* @return if < 0 something went wrong
*/
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr = -1) = 0;
virtual ~ChainFkSolverPos(){};
};
//Forward definition
class ChainFkSolverPos {
public:
/**
* Calculate forward position kinematics for a KDL::Chain,
* from joint coordinates to cartesian pose.
*
* @param q_in input joint coordinates
* @param p_out reference to output cartesian pose
*
* @return if < 0 something went wrong
*/
virtual int JntToCart(const JntArray& q_in, Frame& p_out,int segmentNr=-1)=0;
virtual ~ChainFkSolverPos(){};
};
/**
* \brief This <strong>abstract</strong> class encapsulates a solver
* for the forward velocity kinematics for a KDL::Chain.
*
* @ingroup KinematicFamily
*/
class ChainFkSolverVel {
public:
/**
* Calculate forward position and velocity kinematics, from
* joint coordinates to cartesian coordinates.
*
* @param q_in input joint coordinates (position and velocity)
* @param out output cartesian coordinates (position and velocity)
*
* @return if < 0 something went wrong
*/
virtual int JntToCart(const JntArrayVel &q_in, FrameVel &out, int segmentNr = -1) = 0;
/**
* \brief This <strong>abstract</strong> class encapsulates a solver
* for the forward velocity kinematics for a KDL::Chain.
*
* @ingroup KinematicFamily
*/
class ChainFkSolverVel {
public:
/**
* Calculate forward position and velocity kinematics, from
* joint coordinates to cartesian coordinates.
*
* @param q_in input joint coordinates (position and velocity)
* @param out output cartesian coordinates (position and velocity)
*
* @return if < 0 something went wrong
*/
virtual int JntToCart(const JntArrayVel& q_in, FrameVel& out,int segmentNr=-1)=0;
virtual ~ChainFkSolverVel(){};
};
virtual ~ChainFkSolverVel(){};
};
/**
* \brief This <strong>abstract</strong> class encapsulates a solver
* for the forward acceleration kinematics for a KDL::Chain.
*
* @ingroup KinematicFamily
*/
/**
* \brief This <strong>abstract</strong> class encapsulates a solver
* for the forward acceleration kinematics for a KDL::Chain.
*
* @ingroup KinematicFamily
*/
class ChainFkSolverAcc {
public:
/**
* Calculate forward position, velocity and accelaration
* kinematics, from joint coordinates to cartesian coordinates
*
* @param q_in input joint coordinates (position, velocity and
* acceleration
@param out output cartesian coordinates (position, velocity
* and acceleration
*
* @return if < 0 something went wrong
*/
virtual int JntToCart(const JntArrayAcc &q_in, FrameAcc &out, int segmentNr = -1) = 0;
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

57
intern/itasc/kdl/chainfksolverpos_recursive.cpp
View File

@@ -28,30 +28,37 @@
namespace KDL {
ChainFkSolverPos_recursive::ChainFkSolverPos_recursive(const Chain &_chain) : chain(_chain) {}
int ChainFkSolverPos_recursive::JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr)
{
unsigned int segNr = (unsigned int)segmentNr;
if (segmentNr < 0)
segNr = chain.getNrOfSegments();
p_out = Frame::Identity();
if (q_in.rows() != chain.getNrOfJoints())
return -1;
else if (segNr > chain.getNrOfSegments())
return -1;
else {
int j = 0;
for (unsigned int i = 0; i < segNr; i++) {
p_out = p_out * chain.getSegment(i).pose(((JntArray &)q_in)(j));
j += chain.getSegment(i).getJoint().getNDof();
ChainFkSolverPos_recursive::ChainFkSolverPos_recursive(const Chain& _chain):
chain(_chain)
{
}
return 0;
}
int ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr)
{
unsigned int segNr = (unsigned int)segmentNr;
if(segmentNr<0)
segNr=chain.getNrOfSegments();
p_out = Frame::Identity();
if(q_in.rows()!=chain.getNrOfJoints())
return -1;
else if(segNr>chain.getNrOfSegments())
return -1;
else{
int j=0;
for(unsigned int i=0;i<segNr;i++){
p_out = p_out*chain.getSegment(i).pose(((JntArray&)q_in)(j));
j+=chain.getSegment(i).getJoint().getNDof();
}
return 0;
}
}
ChainFkSolverPos_recursive::~ChainFkSolverPos_recursive()
{
}
}
ChainFkSolverPos_recursive::~ChainFkSolverPos_recursive() {}
} // namespace KDL

33
intern/itasc/kdl/chainfksolverpos_recursive.hpp
View File

@@ -26,24 +26,25 @@
namespace KDL {
/**
* Implementation of a recursive forward position kinematics
* algorithm to calculate the position transformation from joint
* space to Cartesian space of a general kinematic chain (KDL::Chain).
*
* @ingroup KinematicFamily
*/
class ChainFkSolverPos_recursive : public ChainFkSolverPos {
public:
ChainFkSolverPos_recursive(const Chain &chain);
~ChainFkSolverPos_recursive();
/**
* Implementation of a recursive forward position kinematics
* algorithm to calculate the position transformation from joint
* space to Cartesian space of a general kinematic chain (KDL::Chain).
*
* @ingroup KinematicFamily
*/
class ChainFkSolverPos_recursive : public ChainFkSolverPos
{
public:
ChainFkSolverPos_recursive(const Chain& chain);
~ChainFkSolverPos_recursive();
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr = -1);
virtual int JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr=-1);
private:
const Chain chain;
};
private:
const Chain chain;
};
} // namespace KDL
}
#endif

99
intern/itasc/kdl/chainjnttojacsolver.cpp
View File

@@ -24,51 +24,60 @@
#include "chainjnttojacsolver.hpp"
namespace KDL {
ChainJntToJacSolver::ChainJntToJacSolver(const Chain &_chain) : chain(_chain) {}
ChainJntToJacSolver::~ChainJntToJacSolver() {}
int ChainJntToJacSolver::JntToJac(const JntArray &q_in, Jacobian &jac)
namespace KDL
{
assert(q_in.rows() == chain.getNrOfJoints() && q_in.rows() == jac.columns());
Frame T_local, T_joint;
T_total = Frame::Identity();
SetToZero(t_local);
int i = chain.getNrOfSegments() - 1;
unsigned int q_nr = chain.getNrOfJoints();
// Lets recursively iterate until we are in the root segment
while (i >= 0) {
const Segment &segment = chain.getSegment(i);
int ndof = segment.getJoint().getNDof();
q_nr -= ndof;
// get the pose of the joint.
T_joint = segment.getJoint().pose(((JntArray &)q_in)(q_nr));
// combine with the tip to have the tip pose
T_local = T_joint * segment.getFrameToTip();
// calculate new T_end:
T_total = T_local * T_total;
for (int dof = 0; dof < ndof; dof++) {
// combine joint rotation with tip position to get a reference frame for the joint
T_joint.p = T_local.p;
// in which the twist can be computed (needed for NDof joint)
t_local = segment.twist(T_joint, 1.0, dof);
// transform the endpoint of the local twist to the global endpoint:
t_local = t_local.RefPoint(T_total.p - T_local.p);
// transform the base of the twist to the endpoint
t_local = T_total.M.Inverse(t_local);
// store the twist in the jacobian:
jac.twists[q_nr + dof] = t_local;
ChainJntToJacSolver::ChainJntToJacSolver(const Chain& _chain):
chain(_chain)
{
}
ChainJntToJacSolver::~ChainJntToJacSolver()
{
}
int ChainJntToJacSolver::JntToJac(const JntArray& q_in,Jacobian& jac)
{
assert(q_in.rows()==chain.getNrOfJoints()&&
q_in.rows()==jac.columns());
Frame T_local, T_joint;
T_total = Frame::Identity();
SetToZero(t_local);
int i=chain.getNrOfSegments()-1;
unsigned int q_nr = chain.getNrOfJoints();
//Lets recursively iterate until we are in the root segment
while (i >= 0) {
const Segment& segment = chain.getSegment(i);
int ndof = segment.getJoint().getNDof();
q_nr -= ndof;
//get the pose of the joint.
T_joint = segment.getJoint().pose(((JntArray&)q_in)(q_nr));
// combine with the tip to have the tip pose
T_local = T_joint*segment.getFrameToTip();
//calculate new T_end:
T_total = T_local * T_total;
for (int dof=0; dof<ndof; dof++) {
// combine joint rotation with tip position to get a reference frame for the joint
T_joint.p = T_local.p;
// in which the twist can be computed (needed for NDof joint)
t_local = segment.twist(T_joint, 1.0, dof);
//transform the endpoint of the local twist to the global endpoint:
t_local = t_local.RefPoint(T_total.p - T_local.p);
//transform the base of the twist to the endpoint
t_local = T_total.M.Inverse(t_local);
//store the twist in the jacobian:
jac.twists[q_nr+dof] = t_local;
}
i--;
}//endwhile
//Change the base of the complete jacobian from the endpoint to the base
changeBase(jac, T_total.M, jac);
return 0;
}
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

65
intern/itasc/kdl/chainjnttojacsolver.hpp
View File

@@ -22,41 +22,44 @@
#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;
};
} // namespace KDL
private:
const Chain chain;
Twist t_local;
Frame T_total;
};
}
#endif

6
intern/itasc/kdl/frameacc.cpp
View File

@@ -14,12 +14,14 @@
* $Name: $
****************************************************************************/
#include "frameacc.hpp"
namespace KDL {
#ifndef KDL_INLINE
# include "frameacc.inl"
#include "frameacc.inl"
#endif
} // namespace KDL
}

368
intern/itasc/kdl/frameacc.hpp
View File

@@ -27,210 +27,240 @@
#ifndef RRFRAMES_H
#define RRFRAMES_H
#include "frames.hpp"
#include "utilities/rall2d.h"
#include "frames.hpp"
namespace KDL {
class TwistAcc;
typedef Rall2d<double, double, double> doubleAcc;
typedef Rall2d<double,double,double> doubleAcc;
class VectorAcc {
public:
Vector p; //!< position vector
Vector v; //!< velocity vector
Vector dv; //!< acceleration vector
public:
VectorAcc() : p(), v(), dv() {}
explicit VectorAcc(const Vector &_p) : p(_p), v(Vector::Zero()), dv(Vector::Zero()) {}
VectorAcc(const Vector &_p, const Vector &_v) : p(_p), v(_v), dv(Vector::Zero()) {}
VectorAcc(const Vector &_p, const Vector &_v, const Vector &_dv) : p(_p), v(_v), dv(_dv) {}
IMETHOD VectorAcc &operator=(const VectorAcc &arg);
IMETHOD VectorAcc &operator=(const Vector &arg);
IMETHOD VectorAcc &operator+=(const VectorAcc &arg);
IMETHOD VectorAcc &operator-=(const VectorAcc &arg);
IMETHOD static VectorAcc Zero();
IMETHOD void ReverseSign();
IMETHOD doubleAcc Norm();
IMETHOD friend VectorAcc operator+(const VectorAcc &r1, const VectorAcc &r2);
IMETHOD friend VectorAcc operator-(const VectorAcc &r1, const VectorAcc &r2);
IMETHOD friend VectorAcc operator+(const Vector &r1, const VectorAcc &r2);
IMETHOD friend VectorAcc operator-(const Vector &r1, const VectorAcc &r2);
IMETHOD friend VectorAcc operator+(const VectorAcc &r1, const Vector &r2);
IMETHOD friend VectorAcc operator-(const VectorAcc &r1, const Vector &r2);
IMETHOD friend VectorAcc operator*(const VectorAcc &r1, const VectorAcc &r2);
IMETHOD friend VectorAcc operator*(const VectorAcc &r1, const Vector &r2);
IMETHOD friend VectorAcc operator*(const Vector &r1, const VectorAcc &r2);
IMETHOD friend VectorAcc operator*(const VectorAcc &r1, double r2);
IMETHOD friend VectorAcc operator*(double r1, const VectorAcc &r2);
IMETHOD friend VectorAcc operator*(const doubleAcc &r1, const VectorAcc &r2);
IMETHOD friend VectorAcc operator*(const VectorAcc &r2, const doubleAcc &r1);
IMETHOD friend VectorAcc operator*(const Rotation &R, const VectorAcc &x);
IMETHOD friend VectorAcc operator/(const VectorAcc &r1, double r2);
IMETHOD friend VectorAcc operator/(const VectorAcc &r2, const doubleAcc &r1);
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 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 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);
};
class RotationAcc {
public:
Rotation R; //!< rotation matrix
Vector w; //!< angular velocity vector
Vector dw; //!< angular acceration vector
public:
RotationAcc() : R(), w() {}
explicit RotationAcc(const Rotation &R_) : R(R_), w(Vector::Zero()) {}
RotationAcc(const Rotation &R_, const Vector &_w, const Vector &_dw) : R(R_), w(_w), dw(_dw) {}
IMETHOD RotationAcc &operator=(const RotationAcc &arg);
IMETHOD RotationAcc &operator=(const Rotation &arg);
IMETHOD static RotationAcc Identity();
IMETHOD RotationAcc Inverse() const;
IMETHOD VectorAcc Inverse(const VectorAcc &arg) const;
IMETHOD VectorAcc Inverse(const Vector &arg) const;
IMETHOD VectorAcc operator*(const VectorAcc &arg) const;
IMETHOD VectorAcc operator*(const Vector &arg) const;
// Rotations
// The SetRot.. functions set the value of *this to the appropriate rotation matrix.
// The Rot... static functions give the value of the appropriate rotation matrix back.
// The DoRot... functions apply a rotation R to *this,such that *this = *this * R.
// IMETHOD void DoRotX(const doubleAcc& angle);
// IMETHOD void DoRotY(const doubleAcc& angle);
// IMETHOD void DoRotZ(const doubleAcc& angle);
// IMETHOD static RRotation RotX(const doubleAcc& angle);
// IMETHOD static RRotation RotY(const doubleAcc& angle);
// IMETHOD static RRotation RotZ(const doubleAcc& angle);
// IMETHOD void SetRot(const Vector& rotaxis,const doubleAcc& angle);
// Along an arbitrary axes. The norm of rotvec is neglected.
// IMETHOD static RotationAcc Rot(const Vector& rotvec,const doubleAcc& angle);
// rotvec has arbitrary norm
// rotation around a constant vector !
// IMETHOD static RotationAcc Rot2(const Vector& rotvec,const doubleAcc& angle);
// rotvec is normalized.
// rotation around a constant vector !
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 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;
// 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;
};
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;
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;
};
// very similar to Wrench class.
class TwistAcc {
public:
VectorAcc vel; //!< translational velocity and its 1st and 2nd derivative
VectorAcc rot; //!< rotational velocity and its 1st and 2nd derivative
public:
TwistAcc() : vel(), rot(){};
TwistAcc(const VectorAcc &_vel, const VectorAcc &_rot) : vel(_vel), rot(_rot){};
IMETHOD TwistAcc &operator-=(const TwistAcc &arg);
IMETHOD TwistAcc &operator+=(const TwistAcc &arg);
IMETHOD friend TwistAcc operator*(const TwistAcc &lhs, double rhs);
IMETHOD friend TwistAcc operator*(double lhs, const TwistAcc &rhs);
IMETHOD friend TwistAcc operator/(const TwistAcc &lhs, double rhs);
IMETHOD friend TwistAcc operator*(const TwistAcc &lhs, const doubleAcc &rhs);
IMETHOD friend TwistAcc operator*(const doubleAcc &lhs, const TwistAcc &rhs);
IMETHOD friend TwistAcc operator/(const TwistAcc &lhs, const doubleAcc &rhs);
IMETHOD friend TwistAcc operator+(const TwistAcc &lhs, const TwistAcc &rhs);
IMETHOD friend TwistAcc operator-(const TwistAcc &lhs, const TwistAcc &rhs);
IMETHOD friend TwistAcc operator-(const TwistAcc &arg);
IMETHOD friend void SetToZero(TwistAcc &v);
static IMETHOD TwistAcc Zero();
IMETHOD void ReverseSign();
//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 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
TwistAcc():vel(),rot() {};
TwistAcc(const VectorAcc& _vel,const VectorAcc& _rot):vel(_vel),rot(_rot) {};
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 TwistAcc& operator-=(const TwistAcc& arg);
IMETHOD TwistAcc& operator+=(const TwistAcc& arg);
IMETHOD Twist GetTwist() const;
IMETHOD Twist GetTwistDot() const;
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;
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

580
intern/itasc/kdl/frames.cpp
View File

@@ -33,333 +33,314 @@
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]);
if (tmp1 >= tmp2) {
tmp2 = fabs(data[2]);
double tmp1;
double tmp2;
tmp1 = fabs(data[0]);
tmp2 = fabs(data[1]);
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]));
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]));
}
}
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]);
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]);
}
}
else {
beta = PI;
gamma = atan2(data[1], -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)
);
}
}
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::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)
);
}
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
@@ -370,37 +351,42 @@ 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
}

1345
intern/itasc/kdl/frames.hpp
View File

@@ -115,6 +115,7 @@
#ifndef KDL_FRAMES_H
#define KDL_FRAMES_H
#include "utilities/kdl-config.h"
#include "utilities/utility.h"
@@ -122,6 +123,8 @@
namespace KDL {
class Vector;
class Rotation;
class Frame;
@@ -131,128 +134,133 @@ 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);
//! Adds 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);
//! Multiply by a scalar
inline Vector &operator*=(double 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
//! Adds a vector from the Vector object itself
inline Vector& operator +=(const Vector& 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);
//! Multiply by a scalar
inline Vector& operator *=(double arg);
//! To have a uniform operator to put an element to zero, for scalar values
//! and for objects.
inline friend void SetToZero(Vector &v);
//! 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
//! @return a zero vector
inline static Vector Zero();
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);
/** 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);
//! To have a uniform operator to put an element to zero, for scalar values
//! and for objects.
inline friend void SetToZero(Vector& v);
//! @return the norm of the vector
double Norm() const;
//! @return a zero vector
inline static Vector Zero();
//! 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);
/** 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);
//! 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 the norm of the vector
double Norm() const;
//! 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;
//! 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;
};
inline Vector Normalize(const Vector &, double eps = epsilon);
inline Vector Normalize(const Vector&, double eps=epsilon);
/**
\brief represents rotations in 3 dimensional space.
@@ -287,234 +295,227 @@ inline Vector Normalize(const Vector &, double eps = epsilon);
\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);
//! 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);
// = 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. rotvec should be normalized.
static Rotation Rot2(const Vector &rotvec, 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);
// make sure the matrix is a pure rotation (no scaling)
void Ortho();
//! Along an arbitrary axes. rotvec should be normalized.
static Rotation Rot2(const Vector& rotvec,double angle);
// make sure the matrix is a pure rotation (no scaling)
void Ortho();
//! Returns a vector with the direction of the equiv. axis
//! and its norm is angle
Vector GetRot() const;
//! Returns a 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 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 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);
//! 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 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 a vector in RPY coordinates, variables are bound by
//! -PI <= roll <= PI
//! -PI <= Yaw <= PI
//! -PI/2 <= PITCH <= PI/2
//!
//! convention : first rotate around X with roll, then around the
//! old Y with pitch, then around old Z with alfa
void GetRPY(double &roll, double &pitch, double &yaw) const;
//! Gives back a rotation matrix specified with EulerZYX convention :
//! First rotate around Z with alfa,
//! then around the new Y with beta, then around
//! new X with gamma.
//!
//! closely related to RPY-convention
inline static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
{
return RPY(Gamma, Beta, Alfa);
}
//! 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);
//! 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);
}
//! 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;
//! Transformation of the base to which the twist is expressed.
//! Complexity : 18M+12A
//! @see Frame*Twist for a transformation that also transforms
//! the velocity reference point.
inline Twist operator*(const Twist &arg) const;
//! Transformation of the base to which the wrench is expressed.
//! Complexity : 18M+12A
//! @see Frame*Wrench for a transformation that also transforms
//! the force reference point.
inline Wrench operator*(const Wrench &arg) const;
//! 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);
}
//! Access to the underlying unitvectors of the rotation matrix
inline Vector UnitX() const
{
return Vector(data[0], data[3], data[6]);
}
//! 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 void UnitX(const Vector &X)
{
data[0] = X(0);
data[3] = X(1);
data[6] = X(2);
}
//! 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 Vector UnitY() const
{
return Vector(data[1], data[4], data[7]);
}
//! 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 void UnitY(const Vector &X)
{
data[1] = X(0);
data[4] = X(1);
data[7] = X(2);
}
//! Access to the underlying unitvectors of the rotation matrix
inline Vector UnitX() const {
return Vector(data[0],data[3],data[6]);
}
//! Access to the underlying unitvectors of the rotation matrix
inline Vector UnitZ() const
{
return Vector(data[2], data[5], data[8]);
}
//! Access to the underlying unitvectors of the rotation matrix
inline void 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 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 Vector UnitY() const {
return Vector(data[1],data[4],data[7]);
}
//! 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 void UnitY(const Vector& X) {
data[1] = X(0);
data[4] = X(1);
data[7] = X(2);
}
//! 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 Vector UnitZ() const {
return Vector(data[2],data[5],data[8]);
}
friend class Frame;
//! 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;
};
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
@@ -523,148 +524,150 @@ bool operator==(const Rotation &a, const Rotation &b);
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:
inline Frame(const Rotation &R, const Vector &V);
public:
//! 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 Frame(const Rotation& R,const Vector& V);
inline void setValue(float *oglmat);
inline void getValue(float *oglmat) const;
//! 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 Frame() {}
//! The copy constructor. Normal copy by value semantics.
inline Frame(const Frame &arg);
inline void setValue(float* oglmat);
inline void getValue(float* oglmat) const;
//! Reads data from an double array
//\TODO should be formulated as a constructor
void Make4x4(double *d);
inline Frame() {}
//! The copy constructor. Normal copy by value semantics.
inline Frame(const Frame& arg);
//! 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);
//! 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) 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);
// = Inverse
//! Gives back inverse transformation of a Frame
inline Frame Inverse() 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;
//! The same as p2=R.Inverse()*p but more efficient.
inline Vector Inverse(const Vector &arg) const;
// = Inverse
//! Gives back inverse transformation of a Frame
inline Frame Inverse() 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 Vector Inverse(const Vector& 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 Wrench Inverse(const Wrench& arg) const;
//! Normal copy-by-value semantics.
inline Frame &operator=(const Frame &arg);
//! The same as p2=R.Inverse()*p but more efficient.
inline Twist Inverse(const Twist& arg) const;
//! Transformation of the base to which the vector
//! is expressed.
inline Vector operator*(const Vector &arg) const;
//! Normal copy-by-value semantics.
inline Frame& operator = (const Frame& arg);
//! 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 the base to which the vector
//! is expressed.
inline Vector operator * (const Vector& 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 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;
//! Composition of two frames.
inline friend Frame operator*(const Frame &lhs, const Frame &rhs);
//! 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;
//! @return the identity transformation Frame(Rotation::Identity(),Vector::Zero()).
inline static Frame Identity();
//! Composition of two frames.
inline friend Frame operator *(const Frame& lhs,const Frame& rhs);
//! The twist <t_this> is expressed wrt the current
//! frame. This frame is integrated into an updated frame with
//! <samplefrequency>. Very simple first order integration rule.
inline void Integrate(const Twist &t_this, double frequency);
//! @return the identity transformation Frame(Rotation::Identity(),Vector::Zero()).
inline static Frame Identity();
/*
// 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);
//! The twist <t_this> is expressed wrt the current
//! frame. This frame is integrated into an updated frame with
//! <samplefrequency>. Very simple first order integration rule.
inline void Integrate(const Twist& t_this,double frequency);
// DH : 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_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);
static Frame DH(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.
//! 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);
static Frame DH(double a,double alpha,double d,double theta);
//! 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);
//! 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);
};
/**
@@ -672,72 +675,76 @@ class Frame {
*
* This class represents a twist. A twist is the combination of translational
* velocity and rotational velocity applied at one point.
*/
*/
class Twist {
public:
Vector vel; //!< The velocity of that point
Vector rot; //!< The rotational velocity of that point.
public:
//! The default constructor initialises to Zero via the constructor of Vector.
Twist() : vel(), rot(){};
public:
Vector vel; //!< The velocity of that point
Vector rot; //!< The rotational velocity of that point.
public:
Twist(const Vector &_vel, const Vector &_rot) : vel(_vel), rot(_rot){};
//! The default constructor initialises to Zero via the constructor of Vector.
Twist():vel(),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);
Twist(const Vector& _vel,const Vector& _rot):vel(_vel),rot(_rot) {};
//! 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;
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);
double operator[](int index) const
{
return this->operator()(index);
}
//! 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)
{
return this->operator()(index);
}
double operator[] ( int index ) const
{
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);
double& operator[] ( int index )
{
return this->operator() ( index );
}
//! @return a zero Twist : Twist(Vector::Zero(),Vector::Zero())
static inline Twist Zero();
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);
//! Reverses the sign of the twist
inline void ReverseSign();
//! Changes the reference point of the twist.
//! The vector v_base_AB is expressed in the same base as the twist
//! The vector v_base_AB is a vector from the old point to
//! the new point.
//!
//! Complexity : 6M+6A
inline Twist RefPoint(const Vector &v_base_AB) const;
//! @return a zero Twist : Twist(Vector::Zero(),Vector::Zero())
static inline Twist Zero();
//! 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);
//! Reverses the sign of the twist
inline void ReverseSign();
//! 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);
//! 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;
// = Friends
friend class Rotation;
friend class Frame;
};
/**
@@ -746,7 +753,7 @@ class Twist {
* 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:
@@ -770,21 +777,20 @@ 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);
@@ -807,12 +813,11 @@ AccelerationTwist& arg);
//! 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
@@ -826,270 +831,278 @@ eps=epsilon);
*
* This class represents a Wrench. A Wrench is the force and torque applied at a point
*/
class Wrench {
public:
Vector force; //!< Force that is applied at the origin of the current ref frame
Vector torque; //!< Torque that is applied at the origin of the current ref frame
public:
//! Does initialise force and torque to zero via the underlying constructor of Vector
Wrench() : force(), torque(){};
Wrench(const Vector &_force, const Vector &_torque) : force(_force), torque(_torque){};
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:
// = Operators
inline Wrench &operator-=(const Wrench &arg);
inline Wrench &operator+=(const Wrench &arg);
//! 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) {};
//! index-based access to components, first force(0..2), then torque(3..5)
inline double &operator()(int i);
// = 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)
//! 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)
inline double& operator()(int i);
double operator[](int index) const
{
return this->operator()(index);
}
//! 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)
{
return this->operator()(index);
}
double operator[] ( int index ) const
{
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);
double& operator[] ( int index )
{
return this->operator() ( index );
}
inline friend Wrench operator+(const Wrench &lhs, const Wrench &rhs);
inline friend Wrench operator-(const Wrench &lhs, const Wrench &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);
//! An unary - operator
inline friend Wrench operator-(const Wrench &arg);
inline friend Wrench operator+(const Wrench& lhs,const Wrench& rhs);
inline friend Wrench operator-(const Wrench& lhs,const Wrench& rhs);
//! Sets the Wrench to Zero, to have a uniform function that sets an object or
//! double to zero.
inline friend void SetToZero(Wrench &v);
//! An unary - operator
inline friend Wrench operator-(const Wrench& arg);
//! @return a zero Wrench
static inline Wrench Zero();
//! Sets the Wrench to Zero, to have a uniform function that sets an object or
//! double to zero.
inline friend void SetToZero(Wrench& v);
//! Reverses the sign of the current Wrench
inline void ReverseSign();
//! @return a zero Wrench
static inline Wrench Zero();
//! 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;
//! Reverses the sign of the current Wrench
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 Wrench &a, const Wrench &b, double eps);
//! 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;
//! 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];
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);
public:
//! Does not initialise to Zero().
Vector2()
{
data[0] = data[1] = 0.0;
}
inline Vector2(double x, double y);
inline Vector2(const Vector2 &arg);
inline Vector2(double *xyz);
inline Vector2(float *xyz);
inline Vector2& operator = ( const Vector2& arg);
inline Vector2 &operator=(const Vector2 &arg);
//! Access to elements, range checked when NDEBUG is not set, from 0..1
inline double operator()(int index) const;
//! Access to elements, range checked when NDEBUG is not set, from 0..1
inline double operator()(int index) const;
//! 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);
//! store vector components in array
inline void GetValue(double* xy) const;
//! 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 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);
friend class Rotation2;
//! 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;
};
//! 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 Rot... static functions give the value of the appropriate rotation matrix bac
inline static Rotation2 Rot(double angle);
//! The SetRot.. functions set the value of *this to the appropriate rotation matrix.
inline void SetRot(double angle);
//! Gets the angle (in radians)
inline double GetRot() const;
//! The Rot... static functions give the value of the appropriate rotation matrix bac
inline static Rotation2 Rot(double angle);
//! 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);
//! 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);
};
//! 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:
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);
public:
//! 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);
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 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 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);
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);
//! 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 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

491
intern/itasc/kdl/frames_io.cpp
View File

@@ -29,290 +29,285 @@
* *
***************************************************************************/
#include "frames_io.hpp"
#include "frames.hpp"
#include "utilities/error.h"
#include "utilities/error_stack.h"
#include "frames.hpp"
#include "frames_io.hpp"
#include <ctype.h>
#include <iostream>
#include <stdlib.h>
#include <ctype.h>
#include <string.h>
#include <iostream>
namespace KDL {
std::ostream &operator<<(std::ostream &os, const Vector &v)
{
os << "[" << std::setw(KDL_FRAME_WIDTH) << v(0) << "," << std::setw(KDL_FRAME_WIDTH) << v(1)
<< "," << std::setw(KDL_FRAME_WIDTH) << v(2) << "]";
return os;
std::ostream& operator << (std::ostream& os,const Vector& v) {
os << "[" << std::setw(KDL_FRAME_WIDTH) << v(0) << "," << std::setw(KDL_FRAME_WIDTH)<<v(1)
<< "," << std::setw(KDL_FRAME_WIDTH) << v(2) << "]";
return os;
}
std::ostream &operator<<(std::ostream &os, const Twist &v)
{
os << "[" << std::setw(KDL_FRAME_WIDTH) << v.vel(0) << "," << std::setw(KDL_FRAME_WIDTH)
<< v.vel(1) << "," << std::setw(KDL_FRAME_WIDTH) << v.vel(2) << ","
<< std::setw(KDL_FRAME_WIDTH) << v.rot(0) << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(1)
<< "," << std::setw(KDL_FRAME_WIDTH) << v.rot(2) << "]";
return os;
std::ostream& operator << (std::ostream& os,const Twist& v) {
os << "[" << std::setw(KDL_FRAME_WIDTH) << v.vel(0)
<< "," << std::setw(KDL_FRAME_WIDTH) << v.vel(1)
<< "," << std::setw(KDL_FRAME_WIDTH) << v.vel(2)
<< "," << std::setw(KDL_FRAME_WIDTH) << v.rot(0)
<< "," << std::setw(KDL_FRAME_WIDTH) << v.rot(1)
<< "," << std::setw(KDL_FRAME_WIDTH) << v.rot(2)
<< "]";
return os;
}
std::ostream &operator<<(std::ostream &os, const Wrench &v)
{
os << "[" << std::setw(KDL_FRAME_WIDTH) << v.force(0) << "," << std::setw(KDL_FRAME_WIDTH)
<< v.force(1) << "," << std::setw(KDL_FRAME_WIDTH) << v.force(2) << ","
<< std::setw(KDL_FRAME_WIDTH) << v.torque(0) << "," << std::setw(KDL_FRAME_WIDTH)
<< v.torque(1) << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(2) << "]";
return os;
std::ostream& operator << (std::ostream& os,const Wrench& v) {
os << "[" << std::setw(KDL_FRAME_WIDTH) << v.force(0)
<< "," << std::setw(KDL_FRAME_WIDTH) << v.force(1)
<< "," << std::setw(KDL_FRAME_WIDTH) << v.force(2)
<< "," << std::setw(KDL_FRAME_WIDTH) << v.torque(0)
<< "," << std::setw(KDL_FRAME_WIDTH) << v.torque(1)
<< "," << std::setw(KDL_FRAME_WIDTH) << v.torque(2)
<< "]";
return os;
}
std::ostream &operator<<(std::ostream &os, const Rotation &R)
{
std::ostream& operator << (std::ostream& os,const Rotation& R) {
#ifdef KDL_ROTATION_PROPERTIES_RPY
double r, p, y;
R.GetRPY(r, p, y);
os << "[RPY]" << endl;
os << "[";
os << std::setw(KDL_FRAME_WIDTH) << r << ",";
os << std::setw(KDL_FRAME_WIDTH) << p << ",";
os << std::setw(KDL_FRAME_WIDTH) << y << "]";
double r,p,y;
R.GetRPY(r,p,y);
os << "[RPY]"<<endl;
os << "[";
os << std::setw(KDL_FRAME_WIDTH) << r << ",";
os << std::setw(KDL_FRAME_WIDTH) << p << ",";
os << std::setw(KDL_FRAME_WIDTH) << y << "]";
#else
# ifdef KDL_ROTATION_PROPERTIES_EULER
double z, y, x;
R.GetEulerZYX(z, y, x);
os << "[EULERZYX]" << endl;
os << "[";
os << std::setw(KDL_FRAME_WIDTH) << z << ",";
os << std::setw(KDL_FRAME_WIDTH) << y << ",";
os << std::setw(KDL_FRAME_WIDTH) << x << "]";
# else
os << "[";
for (int i = 0; i <= 2; i++) {
os << std::setw(KDL_FRAME_WIDTH) << R(i, 0) << "," << std::setw(KDL_FRAME_WIDTH) << R(i, 1)
<< "," << std::setw(KDL_FRAME_WIDTH) << R(i, 2);
if (i < 2)
os << ";" << std::endl << " ";
else
os << "]";
}
# endif
# ifdef KDL_ROTATION_PROPERTIES_EULER
double z,y,x;
R.GetEulerZYX(z,y,x);
os << "[EULERZYX]"<<endl;
os << "[";
os << std::setw(KDL_FRAME_WIDTH) << z << ",";
os << std::setw(KDL_FRAME_WIDTH) << y << ",";
os << std::setw(KDL_FRAME_WIDTH) << x << "]";
# else
os << "[";
for (int i=0;i<=2;i++) {
os << std::setw(KDL_FRAME_WIDTH) << R(i,0) << "," <<
std::setw(KDL_FRAME_WIDTH) << R(i,1) << "," <<
std::setw(KDL_FRAME_WIDTH) << R(i,2);
if (i<2)
os << ";"<< std::endl << " ";
else
os << "]";
}
# endif
#endif
return os;
return os;
}
std::ostream &operator<<(std::ostream &os, const Frame &T)
std::ostream& operator << (std::ostream& os, const Frame& T)
{
os << "[" << T.M << std::endl << T.p << "]";
return os;
os << "[" << T.M << std::endl<< T.p << "]";
return os;
}
std::ostream &operator<<(std::ostream &os, const Vector2 &v)
{
os << "[" << std::setw(KDL_FRAME_WIDTH) << v(0) << "," << std::setw(KDL_FRAME_WIDTH) << v(1)
<< "]";
return os;
std::ostream& operator << (std::ostream& os,const Vector2& v) {
os << "[" << std::setw(KDL_FRAME_WIDTH) << v(0) << "," << std::setw(KDL_FRAME_WIDTH)<<v(1)
<< "]";
return os;
}
// Rotation2 gives back an angle in degrees with the << and >> operators.
std::ostream &operator<<(std::ostream &os, const Rotation2 &R)
{
os << "[" << R.GetRot() * rad2deg << "]";
return os;
std::ostream& operator << (std::ostream& os,const Rotation2& R) {
os << "[" << R.GetRot()*rad2deg << "]";
return os;
}
std::ostream &operator<<(std::ostream &os, const Frame2 &T)
std::ostream& operator << (std::ostream& os, const Frame2& T)
{
os << T.M << T.p;
return os;
os << T.M << T.p;
return os;
}
std::istream &operator>>(std::istream &is, Vector &v)
{
IOTrace("Stream input Vector (vector or ZERO)");
char storage[10];
EatWord(is, "[]", storage, 10);
if (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::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, ']');
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;
}
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;
if (strcmp(storage,"ZERO")==0) {
v = Vector::Zero();
IOTracePop();
return is;
}
throw Error_Frame_Vector_Unexpected_id();
}
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, '[');
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,'[');
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;
EatEnd(is, ']');
IOTracePop();
return is;
}
if (strcmp(storage, "DH") == 0) {
double a, alpha, d, theta;
Eat(is, '[');
is >> a;
Eat(is, ',');
is >> alpha;
Eat(is, ',');
is >> d;
Eat(is, ',');
is >> theta;
EatEnd(is, ']');
T = Frame::DH(a, alpha * deg2rad, d, theta * deg2rad);
IOTracePop();
return is;
}
throw Error_Frame_Frame_Unexpected_id();
return is;
}
std::istream &operator>>(std::istream &is, Vector2 &v)
{
IOTrace("Stream input Vector2");
Eat(is, '[');
is >> v(0);
Eat(is, ',');
is >> v(1);
EatEnd(is, ']');
IOTracePop();
return is;
}
std::istream &operator>>(std::istream &is, Rotation2 &r)
{
IOTrace("Stream input Rotation2");
Eat(is, '[');
double val;
is >> val;
r.Rot(val * deg2rad);
EatEnd(is, ']');
IOTracePop();
return is;
}
std::istream &operator>>(std::istream &is, Frame2 &T)
{
IOTrace("Stream input Frame2");
is >> T.M;
is >> T.p;
IOTracePop();
return is;
}
} // namespace KDL
} // namespace Frame

50
intern/itasc/kdl/frames_io.hpp
View File

@@ -76,35 +76,39 @@
#ifndef FRAMES_IO_H
#define FRAMES_IO_H
#include "frames.hpp"
#include "jacobian.hpp"
#include "jntarray.hpp"
#include "utilities/utility_io.h"
#include "frames.hpp"
#include "jntarray.hpp"
#include "jacobian.hpp"
namespace KDL {
//! width to be used when printing variables out with frames_io.h
//! global variable, can be changed.
//! width to be used when printing variables out with frames_io.h
//! global variable, can be changed.
// I/O to C++ stream.
std::ostream &operator<<(std::ostream &os, const Vector &v);
std::ostream &operator<<(std::ostream &os, const Rotation &R);
std::ostream &operator<<(std::ostream &os, const Frame &T);
std::ostream &operator<<(std::ostream &os, const Twist &T);
std::ostream &operator<<(std::ostream &os, const Wrench &T);
std::ostream &operator<<(std::ostream &os, const Vector2 &v);
std::ostream &operator<<(std::ostream &os, const Rotation2 &R);
std::ostream &operator<<(std::ostream &os, const Frame2 &T);
std::istream &operator>>(std::istream &is, Vector &v);
std::istream &operator>>(std::istream &is, Rotation &R);
std::istream &operator>>(std::istream &is, Frame &T);
std::istream &operator>>(std::istream &os, Twist &T);
std::istream &operator>>(std::istream &os, Wrench &T);
std::istream &operator>>(std::istream &is, Vector2 &v);
std::istream &operator>>(std::istream &is, Rotation2 &R);
std::istream &operator>>(std::istream &is, Frame2 &T);
// 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);
} // namespace KDL
std::istream& operator >> (std::istream& is,Vector& v);
std::istream& operator >> (std::istream& is,Rotation& R);
std::istream& operator >> (std::istream& is,Frame& T);
std::istream& operator >> (std::istream& os,Twist& T);
std::istream& operator >> (std::istream& os,Wrench& T);
std::istream& operator >> (std::istream& is,Vector2& v);
std::istream& operator >> (std::istream& is,Rotation2& R);
std::istream& operator >> (std::istream& is,Frame2& T);
} // namespace Frame
#endif

7
intern/itasc/kdl/framevel.cpp
View File

@@ -14,12 +14,15 @@
* $Name: $
****************************************************************************/
#include "framevel.hpp"
namespace KDL {
#ifndef KDL_INLINE
# include "framevel.inl"
#include "framevel.inl"
#endif
} // namespace KDL
}

533
intern/itasc/kdl/framevel.hpp
View File

@@ -3,9 +3,10 @@
* 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
@@ -20,42 +21,41 @@
#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<double> doubleVel;
IMETHOD doubleVel diff(const doubleVel &a, const doubleVel &b, double dt = 1.0)
{
return doubleVel((b.t - a.t) / dt, (b.grad - a.grad) / dt);
IMETHOD doubleVel 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<KDL::doubleVel> {
typedef double valueType;
typedef KDL::doubleVel derivType;
template <>
struct Traits<KDL::doubleVel> {
typedef double valueType;
typedef KDL::doubleVel derivType;
};
namespace KDL {
@@ -71,117 +71,113 @@ 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;
}
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);
Rotation value() const { return R;}
Vector deriv() const { return w;}
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 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;
};
class FrameVel
// = TITLE
// An FrameVel is a Frame and its first derivative, a Twist vector
@@ -190,213 +186,208 @@ class FrameVel
// = CAVEATS
//
{
public:
RotationVel M;
VectorVel p;
public:
RotationVel M;
VectorVel p;
public:
FrameVel(){}
public:
FrameVel() {}
explicit FrameVel(const Frame& T_):
M(T_.M),p(T_.p) {}
explicit FrameVel(const Frame &T_) : M(T_.M), p(T_.p) {}
FrameVel(const Frame& T_,const Twist& _t):
M(T_.M,_t.rot),p(T_.p,_t.vel) {}
FrameVel(const 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) {}
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 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 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;
};
// 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:
VectorVel vel;
VectorVel rot;
public:
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){};
// = 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();
// = Reverse Sign
IMETHOD void ReverseSign();
// = Zero
static IMETHOD TwistVel Zero();
// = 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
// = Reverse Sign
IMETHOD void ReverseSign();
// = 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);
// = 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;
// = 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 KDL
} // namespace
#endif

26
intern/itasc/kdl/inertia.cpp
View File

@@ -29,19 +29,23 @@
namespace KDL {
using namespace Eigen;
Inertia::Inertia(double m, double Ixx, double Iyy, double Izz, double Ixy, double Ixz, double Iyz)
: data(Matrix<double, 6, 6>::Zero())
Inertia::Inertia(double m,double Ixx,double Iyy,double Izz,double Ixy,double Ixz,double Iyz):
data(Matrix<double,6,6>::Zero())
{
data(0, 0) = Ixx;
data(1, 1) = Iyy;
data(2, 2) = Izz;
data(2, 1) = data(1, 2) = Ixy;
data(3, 1) = data(1, 3) = Ixz;
data(3, 2) = data(2, 3) = Iyz;
data(0,0)=Ixx;
data(1,1)=Iyy;
data(2,2)=Izz;
data(2,1)=data(1,2)=Ixy;
data(3,1)=data(1,3)=Ixz;
data(3,2)=data(2,3)=Iyz;
data.block(3, 3, 3, 3) = m * Matrix<double, 3, 3>::Identity();
data.block(3,3,3,3)=m*Matrix<double,3,3>::Identity();
}
Inertia::~Inertia() {}
Inertia::~Inertia()
{
}
} // namespace KDL
}

54
intern/itasc/kdl/inertia.hpp
View File

@@ -22,8 +22,8 @@
#ifndef KDLINERTIA_HPP
#define KDLINERTIA_HPP
#include "frames.hpp"
#include <Eigen/Core>
#include "frames.hpp"
namespace KDL {
@@ -34,39 +34,37 @@ 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:
/**
* 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);
class Inertia{
public:
static inline Inertia Zero()
{
return Inertia(0, 0, 0, 0, 0, 0, 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);
friend class Rotation;
friend class Frame;
static inline Inertia Zero(){
return Inertia(0,0,0,0,0,0,0);
};
/**
* F = m*a
*/
// Wrench operator* (const AccelerationTwist& acc);
friend class Rotation;
friend class Frame;
~Inertia();
/**
* F = m*a
*/
// Wrench operator* (const AccelerationTwist& acc);
~Inertia();
private:
Matrix<double,6,6,RowMajor> data;
private:
Matrix<double, 6, 6, RowMajor> data;
};
} // namespace KDL
}
#endif

179
intern/itasc/kdl/jacobian.cpp
View File

@@ -24,106 +24,109 @@
#include "jacobian.hpp"
namespace KDL {
Jacobian::Jacobian(unsigned int _size, unsigned int _nr_blocks)
: size(_size), nr_blocks(_nr_blocks)
namespace KDL
{
twists = new Twist[size * 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 * nr_blocks; i++)
twists[i] = arg.twists[i];
}
Jacobian::Jacobian(const Jacobian& arg):
size(arg.columns()),
nr_blocks(arg.nr_blocks)
{
twists = new Twist[size*nr_blocks];
for(unsigned int i=0;i<size*nr_blocks;i++)
twists[i] = arg.twists[i];
}
Jacobian &Jacobian::operator=(const Jacobian &arg)
{
assert(size == arg.size);
assert(nr_blocks == arg.nr_blocks);
for (unsigned int i = 0; i < size; i++)
twists[i] = arg.twists[i];
return *this;
}
Jacobian& Jacobian::operator = (const Jacobian& arg)
{
assert(size==arg.size);
assert(nr_blocks==arg.nr_blocks);
for(unsigned int i=0;i<size;i++)
twists[i]=arg.twists[i];
return *this;
}
Jacobian::~Jacobian()
{
delete[] twists;
}
double Jacobian::operator()(int i, int j) const
{
assert(i < 6 * (int)nr_blocks && j < (int)size);
return twists[j + 6 * (int)(floor((double)i / 6))](i % 6);
}
Jacobian::~Jacobian()
{
delete [] twists;
}
double &Jacobian::operator()(int i, int j)
{
assert(i < 6 * (int)nr_blocks && j < (int)size);
return twists[j + 6 * (int)(floor((double)i / 6))](i % 6);
}
double Jacobian::operator()(int i,int j)const
{
assert(i<6*(int)nr_blocks&&j<(int)size);
return twists[j+6*(int)(floor((double)i/6))](i%6);
}
unsigned int Jacobian::rows() const
{
return 6 * nr_blocks;
}
double& Jacobian::operator()(int i,int j)
{
assert(i<6*(int)nr_blocks&&j<(int)size);
return twists[j+6*(int)(floor((double)i/6))](i%6);
}
unsigned int Jacobian::columns() const
{
return size;
}
unsigned int Jacobian::rows()const
{
return 6*nr_blocks;
}
void SetToZero(Jacobian &jac)
{
for (unsigned int i = 0; i < jac.size * jac.nr_blocks; i++)
SetToZero(jac.twists[i]);
}
unsigned int Jacobian::columns()const
{
return size;
}
void changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest)
{
assert(src1.size == dest.size);
assert(src1.nr_blocks == dest.nr_blocks);
for (unsigned int i = 0; i < src1.size * src1.nr_blocks; i++)
dest.twists[i] = src1.twists[i].RefPoint(base_AB);
}
void SetToZero(Jacobian& jac)
{
for(unsigned int i=0;i<jac.size*jac.nr_blocks;i++)
SetToZero(jac.twists[i]);
}
void changeBase(const Jacobian &src1, const Rotation &rot, Jacobian &dest)
{
assert(src1.size == dest.size);
assert(src1.nr_blocks == dest.nr_blocks);
for (unsigned int i = 0; i < src1.size * src1.nr_blocks; i++)
dest.twists[i] = rot * src1.twists[i];
}
void changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest)
{
assert(src1.size==dest.size);
assert(src1.nr_blocks==dest.nr_blocks);
for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++)
dest.twists[i]=src1.twists[i].RefPoint(base_AB);
}
void changeRefFrame(const Jacobian &src1, const Frame &frame, Jacobian &dest)
{
assert(src1.size == dest.size);
assert(src1.nr_blocks == dest.nr_blocks);
for (unsigned int i = 0; i < src1.size * src1.nr_blocks; i++)
dest.twists[i] = frame * src1.twists[i];
}
void changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest)
{
assert(src1.size==dest.size);
assert(src1.nr_blocks==dest.nr_blocks);
for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++)
dest.twists[i]=rot*src1.twists[i];
}
bool Jacobian::operator==(const Jacobian &arg)
{
return Equal((*this), arg);
}
void changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest)
{
assert(src1.size==dest.size);
assert(src1.nr_blocks==dest.nr_blocks);
for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++)
dest.twists[i]=frame*src1.twists[i];
}
bool Jacobian::operator!=(const Jacobian &arg)
{
return !Equal((*this), arg);
bool Jacobian::operator ==(const Jacobian& arg)
{
return Equal((*this),arg);
}
bool Jacobian::operator!=(const Jacobian& arg)
{
return !Equal((*this),arg);
}
bool Equal(const Jacobian& a,const Jacobian& b,double eps)
{
if(a.rows()==b.rows()&&a.columns()==b.columns()){
bool rc=true;
for(unsigned int i=0;i<a.columns();i++)
rc&=Equal(a.twists[i],b.twists[i],eps);
return rc;
}else
return false;
}
}
bool Equal(const Jacobian &a, const Jacobian &b, double eps)
{
if (a.rows() == b.rows() && a.columns() == b.columns()) {
bool rc = true;
for (unsigned int i = 0; i < a.columns(); i++)
rc &= Equal(a.twists[i], b.twists[i], eps);
return rc;
}
else
return false;
}
} // namespace KDL

61
intern/itasc/kdl/jacobian.hpp
View File

@@ -24,43 +24,46 @@
#include "frames.hpp"
namespace KDL {
// Forward declaration
class ChainJntToJacSolver;
namespace KDL
{
//Forward declaration
class ChainJntToJacSolver;
class Jacobian {
friend class ChainJntToJacSolver;
class Jacobian
{
friend class ChainJntToJacSolver;
private:
unsigned int size;
unsigned int nr_blocks;
public:
Twist* twists;
Jacobian(unsigned int size,unsigned int nr=1);
Jacobian(const Jacobian& arg);
private:
unsigned int size;
unsigned int nr_blocks;
Jacobian& operator=(const Jacobian& arg);
public:
Twist *twists;
Jacobian(unsigned int size, unsigned int nr = 1);
Jacobian(const Jacobian &arg);
bool operator ==(const Jacobian& arg);
bool operator !=(const Jacobian& arg);
friend bool Equal(const Jacobian& a,const Jacobian& b,double eps);
Jacobian &operator=(const Jacobian &arg);
~Jacobian();
bool operator==(const Jacobian &arg);
bool operator!=(const Jacobian &arg);
double operator()(int i,int j)const;
double& operator()(int i,int j);
unsigned int rows()const;
unsigned int columns()const;
friend bool Equal(const Jacobian &a, const Jacobian &b, double eps);
friend void SetToZero(Jacobian& jac);
~Jacobian();
friend void changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest);
friend void changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest);
friend void changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest);
double operator()(int i, int j) const;
double &operator()(int i, int j);
unsigned int rows() const;
unsigned int columns() const;
friend void SetToZero(Jacobian &jac);
friend void changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest);
friend void changeBase(const Jacobian &src1, const Rotation &rot, Jacobian &dest);
friend void changeRefFrame(const Jacobian &src1, const Frame &frame, Jacobian &dest);
};
bool Equal(const Jacobian &, const Jacobian &, double = epsilon);
} // namespace KDL
};
bool Equal(const Jacobian&, const Jacobian&, double = epsilon);
}
#endif

252
intern/itasc/kdl/jntarray.cpp
View File

@@ -24,129 +24,137 @@
#include "jntarray.hpp"
namespace KDL {
JntArray::JntArray() : size(0), data(NULL) {}
JntArray::JntArray(unsigned int _size) : size(_size)
namespace KDL
{
assert(0 < size);
data = new double[size];
SetToZero(*this);
JntArray::JntArray():
size(0),
data(NULL)
{
}
JntArray::JntArray(unsigned int _size):
size(_size)
{
assert(0 < size);
data = new double[size];
SetToZero(*this);
}
JntArray::JntArray(const JntArray& arg):
size(arg.size)
{
data = ((0 < size) ? new double[size] : NULL);
for(unsigned int i=0;i<size;i++)
data[i]=arg.data[i];
}
JntArray& JntArray::operator = (const JntArray& arg)
{
assert(size==arg.size);
for(unsigned int i=0;i<size;i++)
data[i]=arg.data[i];
return *this;
}
JntArray::~JntArray()
{
delete [] data;
}
void JntArray::resize(unsigned int newSize)
{
delete [] data;
size = newSize;
data = new double[size];
SetToZero(*this);
}
double JntArray::operator[](unsigned int i)const
{
assert(i<size);
return data[i];
}
double& JntArray::operator[](unsigned int i)
{
assert(i<size);
return data[i];
}
double* JntArray::operator()(unsigned int i)
{
if (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);};
}
JntArray::JntArray(const JntArray &arg) : size(arg.size)
{
data = ((0 < size) ? new double[size] : NULL);
for (unsigned int i = 0; i < size; i++)
data[i] = arg.data[i];
}
JntArray &JntArray::operator=(const JntArray &arg)
{
assert(size == arg.size);
for (unsigned int i = 0; i < size; i++)
data[i] = arg.data[i];
return *this;
}
JntArray::~JntArray()
{
delete[] data;
}
void JntArray::resize(unsigned int newSize)
{
delete[] data;
size = newSize;
data = new double[size];
SetToZero(*this);
}
double JntArray::operator[](unsigned int i) const
{
assert(i < size);
return data[i];
}
double &JntArray::operator[](unsigned int i)
{
assert(i < size);
return data[i];
}
double *JntArray::operator()(unsigned int i)
{
if (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

363
intern/itasc/kdl/jntarray.hpp
View File

@@ -25,198 +25,199 @@
#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;
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;
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);
/**
* 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);
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;
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);
/**
* 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

279
intern/itasc/kdl/jntarrayacc.cpp
View File

@@ -24,145 +24,150 @@
#include "jntarrayacc.hpp"
namespace KDL {
JntArrayAcc::JntArrayAcc(unsigned int size) : q(size), qdot(size), qdotdot(size) {}
JntArrayAcc::JntArrayAcc(const JntArray &qin, const JntArray &qdotin, const JntArray &qdotdotin)
: q(qin), qdot(qdotin), qdotdot(qdotdotin)
namespace KDL
{
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()) {}
JntArrayAcc::JntArrayAcc(unsigned int size):
q(size),qdot(size),qdotdot(size)
{
}
JntArrayAcc::JntArrayAcc(const JntArray& qin, const JntArray& qdotin,const JntArray& qdotdotin):
q(qin),qdot(qdotin),qdotdot(qdotdotin)
{
assert(q.rows()==qdot.rows()&&qdot.rows()==qdotdot.rows());
}
JntArrayAcc::JntArrayAcc(const JntArray& qin, const JntArray& qdotin):
q(qin),qdot(qdotin),qdotdot(q.rows())
{
assert(q.rows()==qdot.rows());
}
JntArrayAcc::JntArrayAcc(const JntArray& qin):
q(qin),qdot(q.rows()),qdotdot(q.rows())
{
}
JntArray JntArrayAcc::value() const
{
return q;
JntArray JntArrayAcc::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::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

69
intern/itasc/kdl/jntarrayacc.hpp
View File

@@ -22,45 +22,46 @@
#ifndef KDL_JNTARRAYACC_HPP
#define KDL_JNTARRAYACC_HPP
#include "frameacc.hpp"
#include "utilities/utility.h"
#include "jntarray.hpp"
#include "jntarrayvel.hpp"
#include "utilities/utility.h"
#include "frameacc.hpp"
namespace KDL {
class JntArrayAcc {
public:
JntArray q;
JntArray qdot;
JntArray qdotdot;
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);
public:
JntArrayAcc(unsigned int size);
JntArrayAcc(const JntArray &q, const JntArray &qdot, const JntArray &qdotdot);
JntArrayAcc(const JntArray &q, const JntArray &qdot);
JntArrayAcc(const JntArray &q);
JntArray value()const;
JntArray deriv()const;
JntArray dderiv()const;
JntArray value() const;
JntArray deriv() const;
JntArray dderiv() const;
friend void Add(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest);
friend void Add(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest);
friend void Add(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest);
friend void Subtract(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest);
friend void Subtract(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest);
friend void Subtract(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest);
friend void Multiply(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest);
friend void Multiply(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest);
friend void Multiply(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest);
friend void Divide(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest);
friend void Divide(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest);
friend void Divide(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest);
friend void SetToZero(JntArrayAcc& array);
friend bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps);
};
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
bool Equal(const JntArrayAcc&, const JntArrayAcc&, double = epsilon);
}
#endif

155
intern/itasc/kdl/jntarrayvel.cpp
View File

@@ -22,82 +22,93 @@
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#include "jntarrayacc.hpp"
namespace KDL {
JntArrayVel::JntArrayVel(unsigned int size) : q(size), qdot(size) {}
JntArrayVel::JntArrayVel(const JntArray &qin, const JntArray &qdotin) : q(qin), qdot(qdotin)
namespace KDL
{
assert(q.rows() == qdot.rows());
}
JntArrayVel::JntArrayVel(const JntArray &qin) : q(qin), qdot(q.rows()) {}
JntArrayVel::JntArrayVel(unsigned int size):
q(size),qdot(size)
{
}
JntArrayVel::JntArrayVel(const JntArray& qin, const JntArray& qdotin):
q(qin),qdot(qdotin)
{
assert(q.rows()==qdot.rows());
}
JntArrayVel::JntArrayVel(const JntArray& qin):
q(qin),qdot(q.rows())
{
}
JntArray JntArrayVel::value() const
{
return q;
JntArray JntArrayVel::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::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

55
intern/itasc/kdl/jntarrayvel.hpp
View File

@@ -22,38 +22,39 @@
#ifndef KDL_JNTARRAYVEL_HPP
#define KDL_JNTARRAYVEL_HPP
#include "framevel.hpp"
#include "jntarray.hpp"
#include "utilities/utility.h"
#include "jntarray.hpp"
#include "framevel.hpp"
namespace KDL {
namespace KDL
{
class JntArrayVel {
public:
JntArray q;
JntArray qdot;
class JntArrayVel
{
public:
JntArray q;
JntArray qdot;
public:
JntArrayVel(unsigned int size);
JntArrayVel(const JntArray& q,const JntArray& qdot);
JntArrayVel(const JntArray& q);
public:
JntArrayVel(unsigned int size);
JntArrayVel(const JntArray &q, const JntArray &qdot);
JntArrayVel(const JntArray &q);
JntArray value()const;
JntArray deriv()const;
JntArray value() const;
JntArray deriv() const;
friend void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest);
friend void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest);
friend void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest);
friend void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest);
friend void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest);
friend void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest);
friend void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest);
friend void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest);
friend void SetToZero(JntArrayVel& array);
friend bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps);
};
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
bool Equal(const JntArrayVel&, const JntArrayVel&, double = epsilon);
}
#endif

266
intern/itasc/kdl/joint.cpp
View File

@@ -26,149 +26,139 @@
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() {}
Frame Joint::pose(const double *q) const
{
Joint::~Joint()
{
}
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;
}
}
Frame Joint::pose(const double* q)const
{
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;
}
}
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;
}
}
unsigned int Joint::getNDof() const
{
switch (type) {
case Sphere:
return 3;
case Swing:
return 2;
case None:
return 0;
default:
return 1;
}
}
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
} // end of namespace KDL

204
intern/itasc/kdl/joint.hpp
View File

@@ -27,116 +27,112 @@
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

113
intern/itasc/kdl/kinfam_io.cpp
View File

@@ -26,92 +26,79 @@
#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<<root->first<<": "<<root->second.segment<<"\n";
os << root->first << "(q_nr: " << root->second.q_nr << ")"
<< "\n \t";
for (unsigned int i = 0; i < root->second.children.size(); i++) {
os << (root->second.children[i]) << "\t";
}
return os << "\n";
std::ostream& operator <<(std::ostream& os, SegmentMap::const_iterator root) {
//os<<root->first<<": "<<root->second.segment<<"\n";
os << root->first<<"(q_nr: "<<root->second.q_nr<<")"<<"\n \t";
for (unsigned int i = 0; i < root->second.children.size(); i++) {
os <<(root->second.children[i])<<"\t";
}
return os << "\n";
}
std::istream &operator>>(std::istream &is, Tree &tree)
{
return is;
std::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::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::istream &operator>>(std::istream &is, Jacobian &jac)
{
return is;
}
} // namespace KDL

63
intern/itasc/kdl/kinfam_io.hpp
View File

@@ -22,46 +22,49 @@
#ifndef KDL_KINFAM_IO_HPP
#define KDL_KINFAM_IO_HPP
#include <fstream>
#include <iostream>
#include <fstream>
#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<typename T> std::ostream &operator<<(std::ostream &os, const std::vector<T> &vec)
{
os << "[";
for (unsigned int i = 0; i < vec.size(); i++)
os << vec[i] << " ";
os << "]";
return os;
};
template<typename T>
std::ostream& operator<<(std::ostream& os, const std::vector<T>& vec) {
os << "[";
for (unsigned int i = 0; i < vec.size(); i++)
os << vec[i] << " ";
os << "]";
return os;
}
;
template<typename T> std::istream &operator>>(std::istream &is, std::vector<T> &vec)
{
return is;
};
} // namespace KDL
template<typename T>
std::istream& operator >>(std::istream& is, std::vector<T>& vec) {
return is;
}
;
}
#endif

68
intern/itasc/kdl/segment.cpp
View File

@@ -24,40 +24,48 @@
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 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 Frame &f, const double &qdot, int dof) const
{
return (f.M * joint.twist(qdot, dof)).RefPoint(f.p);
}
} // end of namespace KDL

217
intern/itasc/kdl/segment.hpp
View File

@@ -19,6 +19,7 @@
// 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
@@ -29,122 +30,120 @@
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;
/**
* \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;
private:
Inertia M;
Joint joint;
Frame f_tip;
public:
/**
* Constructor of the segment
*
* @param joint joint of the segment, default:
* Joint(Joint::None)
* @param f_tip frame from the end of the joint to the tip of
* the segment, default: Frame::Identity()
* @param M rigid body inertia of the segment, default: Inertia::Zero()
*/
Segment(const Joint& joint=Joint(), const Frame& f_tip=Frame::Identity(),const Inertia& M = Inertia::Zero());
Segment(const Segment& in);
Segment& operator=(const Segment& arg);
public:
/**
* Constructor of the segment
*
* @param joint joint of the segment, default:
* Joint(Joint::None)
* @param f_tip frame from the end of the joint to the tip of
* the segment, default: Frame::Identity()
* @param M rigid body inertia of the segment, default: Inertia::Zero()
*/
Segment(const Joint &joint = Joint(),
const Frame &f_tip = Frame::Identity(),
const Inertia &M = Inertia::Zero());
Segment(const Segment &in);
Segment &operator=(const Segment &arg);
virtual ~Segment();
virtual ~Segment();
/**
* Request the pose of the segment, given the joint position q.
*
* @param q 1D position of the joint
*
* @return pose from the root to the tip of the segment
*/
Frame pose(const double* q)const;
/**
* Request the 6D-velocity of the tip of the segment, given
* the joint position q and the joint velocity qdot.
*
* @param q ND position of the joint
* @param qdot ND velocity of the joint
*
* @return 6D-velocity of the tip of the segment, expressed
*in the base-frame of the segment(root) and with the tip of
*the segment as reference point.
*/
Twist twist(const double* q,const double& qdot, int dof=0)const;
/**
* Request the pose of the segment, given the joint position q.
*
* @param q 1D position of the joint
*
* @return pose from the root to the tip of the segment
*/
Frame pose(const double *q) const;
/**
* Request the 6D-velocity of the tip of the segment, given
* the joint position q and the joint velocity qdot.
*
* @param q ND position of the joint
* @param qdot ND velocity of the joint
*
* @return 6D-velocity of the tip of the segment, expressed
*in the base-frame of the segment(root) and with the tip of
*the segment as reference point.
*/
Twist twist(const double *q, const double &qdot, int dof = 0) const;
/**
* Request the 6D-velocity at a given point p, relative to base frame of the segment
* givven the joint velocity qdot.
*
* @param p reference point
* @param qdot ND velocity of the joint
*
* @return 6D-velocity at a given point p, expressed
* in the base-frame of the segment(root)
*/
Twist twist(const Vector& p, const double& qdot, int dof=0)const;
/**
* Request the 6D-velocity at a given point p, relative to base frame of the segment
* givven the joint velocity qdot.
*
* @param p reference point
* @param qdot ND velocity of the joint
*
* @return 6D-velocity at a given point p, expressed
* in the base-frame of the segment(root)
*/
Twist twist(const Vector &p, const double &qdot, int dof = 0) const;
/**
* Request the 6D-velocity at a given frame origin, relative to base frame of the segment
* assuming the frame rotation is the rotation of the joint.
*
* @param f joint pose frame + reference point
* @param qdot ND velocity of the joint
*
* @return 6D-velocity at frame reference point, expressed
* in the base-frame of the segment(root)
*/
Twist twist(const Frame& f, const double& qdot, int dof)const;
/**
* Request the 6D-velocity at a given frame origin, relative to base frame of the segment
* assuming the frame rotation is the rotation of the joint.
*
* @param f joint pose frame + reference point
* @param qdot ND velocity of the joint
*
* @return 6D-velocity at frame reference point, expressed
* in the base-frame of the segment(root)
*/
Twist twist(const Frame &f, const double &qdot, int dof) const;
/**
* Request the joint of the segment
*
*
* @return const reference to the joint of the segment
*/
const Joint& getJoint()const
{
return joint;
}
/**
* Request the inertia of the segment
*
*
* @return const reference to the inertia of the segment
*/
const Inertia& getInertia()const
{
return M;
}
/**
* Request the joint of the segment
*
*
* @return const reference to the joint of the segment
*/
const Joint &getJoint() const
{
return joint;
}
/**
* Request the inertia of the segment
*
*
* @return const reference to the inertia of the segment
*/
const Inertia &getInertia() const
{
return M;
}
/**
* Request the pose from the joint end to the tip of the
*segment.
*
* @return const reference to the joint end - segment tip pose.
*/
const Frame& getFrameToTip()const
{
return f_tip;
}
/**
* 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
};
}//end of namespace KDL
#endif

164
intern/itasc/kdl/tree.cpp
View File

@@ -27,104 +27,100 @@
namespace KDL {
using namespace std;
Tree::Tree() : nrOfJoints(0), nrOfSegments(0)
{
TreeElement root;
std::pair<std::string, TreeElement> val("root", root);
segments.insert(val);
Tree::Tree() :
nrOfJoints(0), nrOfSegments(0) {
TreeElement root;
std::pair<std::string, TreeElement> val("root", root);
segments.insert(val);
}
Tree::Tree(const Tree &in)
{
segments.clear();
nrOfSegments = 0;
nrOfJoints = 0;
TreeElement root;
std::pair<std::string, TreeElement> 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<std::string, TreeElement> 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<std::string, TreeElement> 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<std::string, TreeElement> 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<SegmentMap::iterator, bool> retval;
// insert new element
TreeElement elem(segment, *parent, nrOfJoints);
std::pair<std::string, TreeElement> 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<SegmentMap::iterator, bool> retval;
//insert new element
TreeElement elem(segment, *parent, nrOfJoints);
std::pair<std::string, TreeElement> 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;
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;
}
else
// If the child could not be added, return false
return false;
}
return true;
return true;
}
}
} // namespace KDL

273
intern/itasc/kdl/tree.hpp
View File

@@ -22,161 +22,156 @@
#ifndef KDL_TREE_HPP
#define KDL_TREE_HPP
#include "chain.hpp"
#include "segment.hpp"
#include "chain.hpp"
#include <Eigen/Core>
#include <map>
#include <string>
#include <map>
#include <Eigen/Core>
namespace KDL {
// Forward declaration
class TreeElement;
// Eigen allocator is needed for alignment of Eigen data types
typedef std::map<std::string,
TreeElement,
std::less<std::string>,
Eigen::aligned_allocator<std::pair<const std::string, TreeElement>>>
SegmentMap;
namespace KDL
{
//Forward declaration
class TreeElement;
// Eigen allocator is needed for alignment of Eigen data types
typedef std::map<std::string,TreeElement, std::less<std::string>, Eigen::aligned_allocator<std::pair<const std::string, TreeElement> > > SegmentMap;
class TreeElement {
public:
TreeElement() : q_nr(0), parent(0){};
class TreeElement
{
public:
TreeElement():q_nr(0),parent(0)
{};
public:
Segment segment;
unsigned int q_nr;
SegmentMap::value_type const *parent;
std::vector<SegmentMap::const_iterator > 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();
};
};
public:
Segment segment;
unsigned int q_nr;
SegmentMap::value_type const *parent;
std::vector<SegmentMap::const_iterator> 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();
};
};
/**
* \brief This class encapsulates a <strong>tree</strong>
* kinematic interconnection structure. It is build out of segments.
*
* @ingroup KinematicFamily
*/
class Tree
{
private:
SegmentMap segments;
unsigned int nrOfJoints;
unsigned int nrOfSegments;
/**
* \brief This class encapsulates a <strong>tree</strong>
* kinematic interconnection structure. It is build out of segments.
*
* @ingroup KinematicFamily
*/
class Tree {
private:
SegmentMap segments;
unsigned int nrOfJoints;
unsigned int nrOfSegments;
bool addTreeRecursive(SegmentMap::const_iterator root, const std::string& tree_name, const std::string& hook_name);
bool addTreeRecursive(SegmentMap::const_iterator root,
const std::string &tree_name,
const std::string &hook_name);
public:
/**
* The constructor of a tree, a new tree is always empty
*/
Tree();
Tree(const Tree& in);
Tree& operator= (const Tree& arg);
public:
/**
* The constructor of a tree, a new tree is always empty
*/
Tree();
Tree(const Tree &in);
Tree &operator=(const Tree &arg);
/**
* Adds a new segment to the end of the segment with
* hook_name as segment_name
*
* @param segment new segment to add
* @param segment_name name of the new segment
* @param hook_name name of the segment to connect this
* segment with.
*
* @return false if hook_name could not be found.
*/
bool addSegment(const Segment& segment, const std::string& segment_name, const std::string& hook_name);
/**
* Adds a new segment to the end of the segment with
* hook_name as segment_name
*
* @param segment new segment to add
* @param segment_name name of the new segment
* @param hook_name name of the segment to connect this
* segment with.
*
* @return false if hook_name could not be found.
*/
bool addSegment(const Segment &segment,
const std::string &segment_name,
const std::string &hook_name);
/**
* Adds a complete chain to the end of the segment with
* hook_name as segment_name. Segment i of
* the chain will get chain_name+".Segment"+i as segment_name.
*
* @param chain Chain to add
* @param chain_name name of the chain
* @param hook_name name of the segment to connect the chain with.
*
* @return false if hook_name could not be found.
*/
bool addChain(const Chain& chain, const std::string& chain_name, const std::string& hook_name);
/**
* Adds a complete chain to the end of the segment with
* hook_name as segment_name. Segment i of
* the chain will get chain_name+".Segment"+i as segment_name.
*
* @param chain Chain to add
* @param chain_name name of the chain
* @param hook_name name of the segment to connect the chain with.
*
* @return false if hook_name could not be found.
*/
bool addChain(const Chain &chain, const std::string &chain_name, const std::string &hook_name);
/**
* Adds a complete tree to the end of the segment with
* hookname as segment_name. The segments of the tree will get
* tree_name+segment_name as segment_name.
*
* @param tree Tree to add
* @param tree_name name of the tree
* @param hook_name name of the segment to connect the tree with
*
* @return false if hook_name could not be found
*/
bool addTree(const Tree& tree, const std::string& tree_name,const std::string& hook_name);
/**
* Adds a complete tree to the end of the segment with
* hookname as segment_name. The segments of the tree will get
* tree_name+segment_name as segment_name.
*
* @param tree Tree to add
* @param tree_name name of the tree
* @param hook_name name of the segment to connect the tree with
*
* @return false if hook_name could not be found
*/
bool addTree(const Tree &tree, const std::string &tree_name, const std::string &hook_name);
/**
* Request the total number of joints in the tree.\n
* <strong> Important:</strong> It is not the same as the
* total number of segments since a segment does not need to have
* a joint.
*
* @return total nr of joints
*/
unsigned int getNrOfJoints()const
{
return nrOfJoints;
};
/**
* Request the total number of joints in the tree.\n
* <strong> Important:</strong> It is not the same as the
* total number of segments since a segment does not need to have
* a joint.
*
* @return total nr of joints
*/
unsigned int getNrOfJoints() const
{
return nrOfJoints;
};
/**
* Request the total number of segments in the tree.
* @return total number of segments
*/
unsigned int getNrOfSegments()const {return nrOfSegments;};
/**
* Request the total number of segments in the tree.
* @return total number of segments
*/
unsigned int getNrOfSegments() const
{
return nrOfSegments;
};
/**
* Request the segment of the tree with name segment_name.
*
* @param segment_name the name of the requested segment
*
* @return constant iterator pointing to the requested segment
*/
SegmentMap::const_iterator getSegment(const std::string& segment_name)const
{
return segments.find(segment_name);
};
/**
* 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);
};
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;
SegmentMap::value_type const *getSegmentPtr(const std::string &segment_name) const
{
SegmentMap::const_iterator it = segments.find(segment_name);
return &*it;
};
if (it == segments.end())
return 0;
const SegmentMap& getSegments()const
{
return segments;
}
return &*it;
};
const SegmentMap &getSegments() const
{
return segments;
}
virtual ~Tree(){};
};
} // namespace KDL
virtual ~Tree(){};
};
}
#endif

122
intern/itasc/kdl/treefksolver.hpp
View File

@@ -26,87 +26,85 @@
#include <string>
#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 <strong>abstract</strong> class encapsulates a
* solver for the forward position kinematics for a KDL::Tree.
*
* @ingroup KinematicFamily
*/
/**
* \brief This <strong>abstract</strong> class encapsulates a
* solver for the forward position kinematics for a KDL::Tree.
*
* @ingroup KinematicFamily
*/
// Forward definition
class TreeFkSolverPos {
public:
/**
* Calculate forward position kinematics for a KDL::Tree,
* from joint coordinates to cartesian pose.
*
* @param q_in input joint coordinates
* @param p_out reference to output cartesian pose
*
* @return if < 0 something went wrong
*/
virtual int JntToCart(const JntArray &q_in,
Frame &p_out,
const std::string &segmentName,
const std::string &baseName) = 0;
virtual ~TreeFkSolverPos(){};
};
//Forward definition
class TreeFkSolverPos {
public:
/**
* Calculate forward position kinematics for a KDL::Tree,
* from joint coordinates to cartesian pose.
*
* @param q_in input joint coordinates
* @param p_out reference to output cartesian pose
*
* @return if < 0 something went wrong
*/
virtual int JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName)=0;
virtual ~TreeFkSolverPos(){};
};
/**
* \brief This <strong>abstract</strong> class encapsulates a solver
* for the forward velocity kinematics for a KDL::Tree.
*
* @ingroup KinematicFamily
*/
/**
* \brief This <strong>abstract</strong> class encapsulates a solver
* for the forward velocity kinematics for a KDL::Tree.
*
* @ingroup KinematicFamily
*/
// class TreeFkSolverVel {
// public:
/**
* Calculate forward position and velocity kinematics, from
* joint coordinates to cartesian coordinates.
*
* @param q_in input joint coordinates (position and velocity)
* @param out output cartesian coordinates (position and velocity)
*
* @return if < 0 something went wrong
*/
/**
* Calculate forward position and velocity kinematics, from
* joint coordinates to cartesian coordinates.
*
* @param q_in input joint coordinates (position and velocity)
* @param out output cartesian coordinates (position and velocity)
*
* @return if < 0 something went wrong
*/
// virtual int JntToCart(const JntArrayVel& q_in, FrameVel& out,int segmentNr=-1)=0;
// virtual ~TreeFkSolverVel(){};
// };
/**
* \brief This <strong>abstract</strong> class encapsulates a solver
* for the forward acceleration kinematics for a KDL::Tree.
*
* @ingroup KinematicFamily
*/
/**
* \brief This <strong>abstract</strong> class encapsulates a solver
* for the forward acceleration kinematics for a KDL::Tree.
*
* @ingroup KinematicFamily
*/
// class TreeFkSolverAcc {
// public:
/**
* Calculate forward position, velocity and accelaration
* kinematics, from joint coordinates to cartesian coordinates
*
* @param q_in input joint coordinates (position, velocity and
* acceleration
@param out output cartesian coordinates (position, velocity
* and acceleration
*
* @return if < 0 something went wrong
*/
/**
* 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

77
intern/itasc/kdl/treefksolverpos_recursive.cpp
View File

@@ -28,44 +28,45 @@
namespace KDL {
TreeFkSolverPos_recursive::TreeFkSolverPos_recursive(const Tree &_tree) : tree(_tree) {}
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()
{
}
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() {}
} // namespace KDL

42
intern/itasc/kdl/treefksolverpos_recursive.hpp
View File

@@ -27,31 +27,27 @@
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;
private:
const Tree tree;
Frame recursiveFk(const JntArray& q_in, SegmentMap::value_type const* it, SegmentMap::value_type const* baseit);
};
Frame recursiveFk(const JntArray &q_in,
SegmentMap::value_type const *it,
SegmentMap::value_type const *baseit);
};
} // namespace KDL
}
#endif

107
intern/itasc/kdl/treejnttojacsolver.cpp
View File

@@ -13,66 +13,69 @@
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; 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);
//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;
return 0;
}//end JntToJac
}//end namespace
} // end JntToJac
} // namespace KDL

27
intern/itasc/kdl/treejnttojacsolver.hpp
View File

@@ -8,30 +8,31 @@
#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);
int JntToJac(const JntArray& q_in, Jacobian& jac,
const std::string& segmentname);
private:
KDL::Tree tree;
private:
KDL::Tree tree;
};
} // namespace KDL
}//End of namespace
#endif /* TREEJNTTOJACSOLVER_H_ */

401
intern/itasc/kdl/utilities/error.h
View File

@@ -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,22 +24,23 @@
* 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"
@@ -47,362 +48,196 @@
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;
}
class Error_IO : public Error {
std::string msg;
int typenr;
public:
Error_IO(const std::string& _msg="Unspecified I/O Error",int typenr=0):msg(_msg) {}
virtual const char* Description() const {return msg.c_str();}
virtual int GetType() const {return typenr;}
};
class Error_BasicIO : public Error_IO {};
class Error_BasicIO_File : public Error_BasicIO {
public:
virtual const char *Description() const
{
return "Error while reading stream";
}
virtual int GetType() const
{
return 1;
}
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

52
intern/itasc/kdl/utilities/error_stack.cpp
View File

@@ -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 <cstring>
#include <stack>
#include <string>
#include <vector>
#include <string>
#include <cstring>
namespace KDL {
// Trace of the call stack of the I/O routines to help user
// interprete error messages from I/O
typedef std::stack<std::string> ErrorStack;
typedef std::stack<std::string> ErrorStack;
ErrorStack errorstack;
// should be in Thread Local Storage if this gets multithreaded one day...
void IOTrace(const std::string &description)
{
errorstack.push(description);
void IOTrace(const std::string& description) {
errorstack.push(description);
}
void IOTracePop()
{
errorstack.pop();
}
void IOTraceOutput(std::ostream &os)
{
while (!errorstack.empty()) {
os << errorstack.top().c_str() << std::endl;
void IOTracePop() {
errorstack.pop();
}
}
void IOTracePopStr(char *buffer, int size)
{
if (errorstack.empty()) {
*buffer = 0;
return;
}
strncpy(buffer, errorstack.top().c_str(), size);
errorstack.pop();
void IOTraceOutput(std::ostream& os) {
while (!errorstack.empty()) {
os << errorstack.top().c_str() << std::endl;
errorstack.pop();
}
}
} // namespace KDL
void IOTracePopStr(char* buffer,int size) {
if (errorstack.empty()) {
*buffer = 0;
return;
}
strncpy(buffer,errorstack.top().c_str(),size);
errorstack.pop();
}
}

27
intern/itasc/kdl/utilities/error_stack.h
View File

@@ -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,12 +24,13 @@
* 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
*/
@@ -40,6 +41,7 @@
#include "utility_io.h"
#include <string>
namespace KDL {
/*
@@ -47,18 +49,21 @@ 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

1
intern/itasc/kdl/utilities/kdl-config.h
View File

@@ -22,6 +22,7 @@
/* 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

755
intern/itasc/kdl/utilities/rall1d.h
View File

@@ -1,461 +1,476 @@
/*****************************************************************************
* \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 "utility.h"
#include <assert.h>
#include "utility.h"
namespace KDL {
/**
* Rall1d contains a value, and its gradient, and defines an algebraic structure on this pair.
* This template class has 3 template parameters :
* - T contains the type of the value.
* - V contains the type of the gradient (can be a vector-like type).
* - S defines a scalar type that can operate on Rall1d. This is the type that
* is used to give back values of Norm() etc.
* - 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<double>, Rall1d<double>, double> ).
*
* S is always passed by value.
* S is always passed by value.
*
* \par Class Type
* Concrete implementation
*/
template<typename T, typename V = T, typename S = T> class Rall1d {
public:
typedef T valuetype;
typedef V gradienttype;
typedef S scalartype;
template <typename T,typename V=T,typename S=T>
class Rall1d
{
public:
typedef T valuetype;
typedef V gradienttype;
typedef S scalartype;
public :
T t; //!< value
V grad; //!< gradient
public :
INLINE Rall1d() {}
public:
T t; //!< value
V grad; //!< gradient
public:
INLINE Rall1d() {}
T value() const {
return t;
}
V deriv() const {
return grad;
}
T value() const
{
return t;
}
V deriv() const
{
return grad;
}
explicit INLINE Rall1d(typename TI<T>::Arg c)
{t=T(c);SetToZero(grad);}
explicit INLINE Rall1d(typename TI<T>::Arg c)
{
t = T(c);
SetToZero(grad);
}
INLINE Rall1d(typename TI<T>::Arg tn, typename TI<V>::Arg afg):t(tn),grad(afg) {}
INLINE Rall1d(typename TI<T>::Arg tn, typename TI<V>::Arg afg) : t(tn), grad(afg) {}
INLINE Rall1d(const Rall1d<T,V,S>& r):t(r.t),grad(r.grad) {}
//if one defines this constructor, it's better optimized then the
//automatically generated one ( this one set's up a loop to copy
// word by word.
INLINE T& Value() {
return t;
}
INLINE Rall1d(const Rall1d<T, V, S> &r) : t(r.t), grad(r.grad) {}
// if one defines this constructor, it's better optimized then the
// automatically generated one ( this one set's up a loop to copy
// word by word.
INLINE V& Gradient() {
return grad;
}
INLINE T &Value()
{
return t;
}
INLINE static Rall1d<T,V,S> Zero() {
Rall1d<T,V,S> tmp;
SetToZero(tmp);
return tmp;
}
INLINE static Rall1d<T,V,S> Identity() {
Rall1d<T,V,S> tmp;
SetToIdentity(tmp);
return tmp;
}
INLINE V &Gradient()
{
return grad;
}
INLINE Rall1d<T,V,S>& operator =(S c)
{t=c;SetToZero(grad);return *this;}
INLINE static Rall1d<T, V, S> Zero()
{
Rall1d<T, V, S> tmp;
SetToZero(tmp);
return tmp;
}
INLINE static Rall1d<T, V, S> Identity()
{
Rall1d<T, V, S> tmp;
SetToIdentity(tmp);
return tmp;
}
INLINE Rall1d<T,V,S>& operator =(const Rall1d<T,V,S>& r)
{t=r.t;grad=r.grad;return *this;}
INLINE Rall1d<T, V, S> &operator=(S c)
{
t = c;
SetToZero(grad);
return *this;
}
INLINE Rall1d<T,V,S>& operator /=(const Rall1d<T,V,S>& rhs)
{
grad = LinComb(rhs.t,grad,-t,rhs.grad) / (rhs.t*rhs.t);
t /= rhs.t;
return *this;
}
INLINE Rall1d<T, V, S> &operator=(const Rall1d<T, V, S> &r)
{
t = r.t;
grad = r.grad;
return *this;
}
INLINE Rall1d<T,V,S>& operator *=(const Rall1d<T,V,S>& rhs)
{
LinCombR(rhs.t,grad,t,rhs.grad,grad);
t *= rhs.t;
return *this;
}
INLINE Rall1d<T, V, S> &operator/=(const Rall1d<T, V, S> &rhs)
{
grad = LinComb(rhs.t, grad, -t, rhs.grad) / (rhs.t * rhs.t);
t /= rhs.t;
return *this;
}
INLINE Rall1d<T,V,S>& operator +=(const Rall1d<T,V,S>& rhs)
{
grad +=rhs.grad;
t +=rhs.t;
return *this;
}
INLINE Rall1d<T, V, S> &operator*=(const Rall1d<T, V, S> &rhs)
{
LinCombR(rhs.t, grad, t, rhs.grad, grad);
t *= rhs.t;
return *this;
}
INLINE Rall1d<T,V,S>& operator -=(const Rall1d<T,V,S>& rhs)
{
grad -= rhs.grad;
t -= rhs.t;
return *this;
}
INLINE Rall1d<T, V, S> &operator+=(const Rall1d<T, V, S> &rhs)
{
grad += rhs.grad;
t += rhs.t;
return *this;
}
INLINE Rall1d<T,V,S>& operator /=(S rhs)
{
grad /= rhs;
t /= rhs;
return *this;
}
INLINE Rall1d<T, V, S> &operator-=(const Rall1d<T, V, S> &rhs)
{
grad -= rhs.grad;
t -= rhs.t;
return *this;
}
INLINE Rall1d<T,V,S>& operator *=(S rhs)
{
grad *= rhs;
t *= rhs;
return *this;
}
INLINE Rall1d<T, V, S> &operator/=(S rhs)
{
grad /= rhs;
t /= rhs;
return *this;
}
INLINE Rall1d<T,V,S>& operator +=(S rhs)
{
t += rhs;
return *this;
}
INLINE Rall1d<T, V, S> &operator*=(S rhs)
{
grad *= rhs;
t *= rhs;
return *this;
}
INLINE Rall1d<T,V,S>& operator -=(S rhs)
{
t -= rhs;
return *this;
}
INLINE Rall1d<T, V, S> &operator+=(S rhs)
{
t += rhs;
return *this;
}
INLINE Rall1d<T, V, S> &operator-=(S rhs)
{
t -= rhs;
return *this;
}
// = operators
/* gives warnings on cygwin
// = operators
/* gives warnings on cygwin
template <class T2,class V2,class S2>
friend INLINE Rall1d<T2,V2,S2> operator /(const Rall1d<T2,V2,S2>& lhs,const Rall1d<T2,V2,S2>& rhs);
friend INLINE Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
friend INLINE Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
friend INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
friend INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> operator *(S s,const Rall1d<T,V,S>& v);
friend INLINE Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& v,S s);
friend INLINE Rall1d<T,V,S> operator +(S s,const Rall1d<T,V,S>& v);
friend INLINE Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& v,S s);
friend INLINE Rall1d<T,V,S> operator -(S s,const Rall1d<T,V,S>& v);
friend INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& v,S s);
friend INLINE Rall1d<T,V,S> operator /(S s,const Rall1d<T,V,S>& v);
friend INLINE Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& v,S s);
template <class T2,class V2,class S2>
friend INLINE Rall1d<T2,V2,S2> operator /(const Rall1d<T2,V2,S2>& lhs,const Rall1d<T2,V2,S2>&
rhs);
// = Mathematical functions that operate on Rall1d objects
friend INLINE Rall1d<T,V,S> exp(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> log(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> sin(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> cos(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> tan(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> sinh(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> cosh(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> sqr(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> pow(const Rall1d<T,V,S>& arg,double m) ;
friend INLINE Rall1d<T,V,S> sqrt(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> atan(const Rall1d<T,V,S>& x);
friend INLINE Rall1d<T,V,S> hypot(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x);
friend INLINE Rall1d<T,V,S> asin(const Rall1d<T,V,S>& x);
friend INLINE Rall1d<T,V,S> acos(const Rall1d<T,V,S>& x);
friend INLINE Rall1d<T,V,S> abs(const Rall1d<T,V,S>& x);
friend INLINE S Norm(const Rall1d<T,V,S>& value) ;
friend INLINE Rall1d<T,V,S> tanh(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> atan2(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x);
// = Utility functions to improve performance
friend INLINE Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
friend INLINE Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
friend INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
friend INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> operator *(S s,const Rall1d<T,V,S>& v);
friend INLINE Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& v,S s);
friend INLINE Rall1d<T,V,S> operator +(S s,const Rall1d<T,V,S>& v);
friend INLINE Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& v,S s);
friend INLINE Rall1d<T,V,S> operator -(S s,const Rall1d<T,V,S>& v);
friend INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& v,S s);
friend INLINE Rall1d<T,V,S> operator /(S s,const Rall1d<T,V,S>& v);
friend INLINE Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& v,S s);
friend INLINE Rall1d<T,V,S> LinComb(S alfa,const Rall1d<T,V,S>& a,
const T& beta,const Rall1d<T,V,S>& b );
friend INLINE void LinCombR(S alfa,const Rall1d<T,V,S>& a,
const T& beta,const Rall1d<T,V,S>& b,Rall1d<T,V,S>& result );
// = Setting value of a Rall1d object to 0 or 1
// = Mathematical functions that operate on Rall1d objects
friend INLINE Rall1d<T,V,S> exp(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> log(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> sin(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> cos(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> tan(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> sinh(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> cosh(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> sqr(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> pow(const Rall1d<T,V,S>& arg,double m) ;
friend INLINE Rall1d<T,V,S> sqrt(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> atan(const Rall1d<T,V,S>& x);
friend INLINE Rall1d<T,V,S> hypot(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x);
friend INLINE Rall1d<T,V,S> asin(const Rall1d<T,V,S>& x);
friend INLINE Rall1d<T,V,S> acos(const Rall1d<T,V,S>& x);
friend INLINE Rall1d<T,V,S> abs(const Rall1d<T,V,S>& x);
friend INLINE S Norm(const Rall1d<T,V,S>& value) ;
friend INLINE Rall1d<T,V,S> tanh(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> atan2(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x);
friend INLINE void SetToZero(Rall1d<T,V,S>& value);
friend INLINE void SetToOne(Rall1d<T,V,S>& value);
// = Equality in an eps-interval
friend INLINE bool Equal(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x,double eps);
*/
};
// = Utility functions to improve performance
friend INLINE Rall1d<T,V,S> LinComb(S alfa,const Rall1d<T,V,S>& a,
const T& beta,const Rall1d<T,V,S>& b );
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
{
return Rall1d<T,V,S>(lhs.t/rhs.t,(lhs.grad*rhs.t-lhs.t*rhs.grad)/(rhs.t*rhs.t));
}
friend INLINE void LinCombR(S alfa,const Rall1d<T,V,S>& a,
const T& beta,const Rall1d<T,V,S>& b,Rall1d<T,V,S>& result );
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
{
return Rall1d<T,V,S>(lhs.t*rhs.t,rhs.t*lhs.grad+lhs.t*rhs.grad);
}
// = Setting value of a Rall1d object to 0 or 1
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
{
return Rall1d<T,V,S>(lhs.t+rhs.t,lhs.grad+rhs.grad);
}
friend INLINE void SetToZero(Rall1d<T,V,S>& value);
friend INLINE void SetToOne(Rall1d<T,V,S>& value);
// = Equality in an eps-interval
friend INLINE bool Equal(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x,double eps);
*/
};
template<class T, class V, class S>
INLINE Rall1d<T, V, S> operator/(const Rall1d<T, V, S> &lhs, const Rall1d<T, V, S> &rhs)
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
{
return Rall1d<T,V,S>(lhs.t-rhs.t,lhs.grad-rhs.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& arg)
{
return Rall1d<T,V,S>(-arg.t,-arg.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator *(S s,const Rall1d<T,V,S>& v)
{
return Rall1d<T,V,S>(s*v.t,s*v.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& v,S s)
{
return Rall1d<T,V,S>(v.t*s,v.grad*s);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator +(S s,const Rall1d<T,V,S>& v)
{
return Rall1d<T,V,S>(s+v.t,v.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& v,S s)
{
return Rall1d<T,V,S>(v.t+s,v.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator -(S s,const Rall1d<T,V,S>& v)
{
return Rall1d<T,V,S>(s-v.t,-v.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& v,S s)
{
return Rall1d<T,V,S>(v.t-s,v.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator /(S s,const Rall1d<T,V,S>& v)
{
return Rall1d<T,V,S>(s/v.t,(-s*v.grad)/(v.t*v.t));
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& v,S s)
{
return Rall1d<T,V,S>(v.t/s,v.grad/s);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> exp(const Rall1d<T,V,S>& arg)
{
T v;
v= (exp(arg.t));
return Rall1d<T,V,S>(v,v*arg.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> log(const Rall1d<T,V,S>& arg)
{
T v;
v=(log(arg.t));
return Rall1d<T,V,S>(v,arg.grad/arg.t);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> sin(const Rall1d<T,V,S>& arg)
{
T v;
v=(sin(arg.t));
return Rall1d<T,V,S>(v,cos(arg.t)*arg.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> cos(const Rall1d<T,V,S>& arg)
{
T v;
v=(cos(arg.t));
return Rall1d<T,V,S>(v,-sin(arg.t)*arg.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> tan(const Rall1d<T,V,S>& arg)
{
T v;
v=(tan(arg.t));
return Rall1d<T,V,S>(v,arg.grad/sqr(cos(arg.t)));
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> sinh(const Rall1d<T,V,S>& arg)
{
T v;
v=(sinh(arg.t));
return Rall1d<T,V,S>(v,cosh(arg.t)*arg.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> cosh(const Rall1d<T,V,S>& arg)
{
T v;
v=(cosh(arg.t));
return Rall1d<T,V,S>(v,sinh(arg.t)*arg.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> sqr(const Rall1d<T,V,S>& arg)
{
T v;
v=(arg.t*arg.t);
return Rall1d<T,V,S>(v,(2.0*arg.t)*arg.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> pow(const Rall1d<T,V,S>& arg,double m)
{
T v;
v=(pow(arg.t,m));
return Rall1d<T,V,S>(v,(m*v/arg.t)*arg.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> sqrt(const Rall1d<T,V,S>& arg)
{
T v;
v=sqrt(arg.t);
return Rall1d<T,V,S>(v, (0.5/v)*arg.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> atan(const Rall1d<T,V,S>& x)
{
return Rall1d<T, V, S>(lhs.t / rhs.t, (lhs.grad * rhs.t - lhs.t * rhs.grad) / (rhs.t * rhs.t));
T v;
v=(atan(x.t));
return Rall1d<T,V,S>(v,x.grad/(1.0+sqr(x.t)));
}
template<class T, class V, class S>
INLINE Rall1d<T, V, S> operator*(const Rall1d<T, V, S> &lhs, const Rall1d<T, V, S> &rhs)
template <class T,class V,class S>
INLINE Rall1d<T,V,S> hypot(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x)
{
return Rall1d<T, V, S>(lhs.t * rhs.t, rhs.t * lhs.grad + lhs.t * rhs.grad);
T v;
v=(hypot(y.t,x.t));
return Rall1d<T,V,S>(v,(x.t/v)*x.grad+(y.t/v)*y.grad);
}
template<class T, class V, class S>
INLINE Rall1d<T, V, S> operator+(const Rall1d<T, V, S> &lhs, const Rall1d<T, V, S> &rhs)
template <class T,class V,class S>
INLINE Rall1d<T,V,S> asin(const Rall1d<T,V,S>& x)
{
return Rall1d<T, V, S>(lhs.t + rhs.t, lhs.grad + rhs.grad);
T v;
v=(asin(x.t));
return Rall1d<T,V,S>(v,x.grad/sqrt(1.0-sqr(x.t)));
}
template<class T, class V, class S>
INLINE Rall1d<T, V, S> operator-(const Rall1d<T, V, S> &lhs, const Rall1d<T, V, S> &rhs)
template <class T,class V,class S>
INLINE Rall1d<T,V,S> acos(const Rall1d<T,V,S>& x)
{
return Rall1d<T, V, S>(lhs.t - rhs.t, lhs.grad - rhs.grad);
T v;
v=(acos(x.t));
return Rall1d<T,V,S>(v,-x.grad/sqrt(1.0-sqr(x.t)));
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> operator-(const Rall1d<T, V, S> &arg)
template <class T,class V,class S>
INLINE Rall1d<T,V,S> abs(const Rall1d<T,V,S>& x)
{
return Rall1d<T, V, S>(-arg.t, -arg.grad);
T v;
v=(Sign(x));
return Rall1d<T,V,S>(v*x,v*x.grad);
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> operator*(S s, const Rall1d<T, V, S> &v)
template <class T,class V,class S>
INLINE S Norm(const Rall1d<T,V,S>& value)
{
return Rall1d<T, V, S>(s * v.t, s * v.grad);
return Norm(value.t);
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> operator*(const Rall1d<T, V, S> &v, S s)
{
return Rall1d<T, V, S>(v.t * s, v.grad * s);
template <class T,class V,class S>
INLINE Rall1d<T,V,S> tanh(const Rall1d<T,V,S>& arg)
{
T v(tanh(arg.t));
return Rall1d<T,V,S>(v,arg.grad/sqr(cosh(arg.t)));
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> operator+(S s, const Rall1d<T, V, S> &v)
template <class T,class V,class S>
INLINE Rall1d<T,V,S> atan2(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x)
{
return Rall1d<T, V, S>(s + v.t, v.grad);
T v(x.t*x.t+y.t*y.t);
return Rall1d<T,V,S>(atan2(y.t,x.t),(x.t*y.grad-y.t*x.grad)/v);
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> operator+(const Rall1d<T, V, S> &v, S s)
{
return Rall1d<T, V, S>(v.t + s, v.grad);
template <class T,class V,class S>
INLINE Rall1d<T,V,S> LinComb(S alfa,const Rall1d<T,V,S>& a,
const T& beta,const Rall1d<T,V,S>& b ) {
return Rall1d<T,V,S>(
LinComb(alfa,a.t,beta,b.t),
LinComb(alfa,a.grad,beta,b.grad)
);
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> operator-(S s, const Rall1d<T, V, S> &v)
{
return Rall1d<T, V, S>(s - v.t, -v.grad);
template <class T,class V,class S>
INLINE void LinCombR(S alfa,const Rall1d<T,V,S>& a,
const T& beta,const Rall1d<T,V,S>& b,Rall1d<T,V,S>& result ) {
LinCombR(alfa, a.t, beta, b.t, result.t);
LinCombR(alfa, a.grad, beta, b.grad, result.grad);
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> operator-(const Rall1d<T, V, S> &v, S s)
template <class T,class V,class S>
INLINE void SetToZero(Rall1d<T,V,S>& value)
{
SetToZero(value.grad);
SetToZero(value.t);
}
template <class T,class V,class S>
INLINE void SetToIdentity(Rall1d<T,V,S>& value)
{
SetToIdentity(value.t);
SetToZero(value.grad);
}
template <class T,class V,class S>
INLINE bool Equal(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x,double eps=epsilon)
{
return Rall1d<T, V, S>(v.t - s, v.grad);
return (Equal(x.t,y.t,eps)&&Equal(x.grad,y.grad,eps));
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> operator/(S s, const Rall1d<T, V, S> &v)
{
return Rall1d<T, V, S>(s / v.t, (-s * v.grad) / (v.t * v.t));
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> operator/(const Rall1d<T, V, S> &v, S s)
{
return Rall1d<T, V, S>(v.t / s, v.grad / s);
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> exp(const Rall1d<T, V, S> &arg)
{
T v;
v = (exp(arg.t));
return Rall1d<T, V, S>(v, v * arg.grad);
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> log(const Rall1d<T, V, S> &arg)
{
T v;
v = (log(arg.t));
return Rall1d<T, V, S>(v, arg.grad / arg.t);
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> sin(const Rall1d<T, V, S> &arg)
{
T v;
v = (sin(arg.t));
return Rall1d<T, V, S>(v, cos(arg.t) * arg.grad);
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> cos(const Rall1d<T, V, S> &arg)
{
T v;
v = (cos(arg.t));
return Rall1d<T, V, S>(v, -sin(arg.t) * arg.grad);
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> tan(const Rall1d<T, V, S> &arg)
{
T v;
v = (tan(arg.t));
return Rall1d<T, V, S>(v, arg.grad / sqr(cos(arg.t)));
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> sinh(const Rall1d<T, V, S> &arg)
{
T v;
v = (sinh(arg.t));
return Rall1d<T, V, S>(v, cosh(arg.t) * arg.grad);
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> cosh(const Rall1d<T, V, S> &arg)
{
T v;
v = (cosh(arg.t));
return Rall1d<T, V, S>(v, sinh(arg.t) * arg.grad);
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> sqr(const Rall1d<T, V, S> &arg)
{
T v;
v = (arg.t * arg.t);
return Rall1d<T, V, S>(v, (2.0 * arg.t) * arg.grad);
}
template<class T, class V, class S>
INLINE Rall1d<T, V, S> pow(const Rall1d<T, V, S> &arg, double m)
{
T v;
v = (pow(arg.t, m));
return Rall1d<T, V, S>(v, (m * v / arg.t) * arg.grad);
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> sqrt(const Rall1d<T, V, S> &arg)
{
T v;
v = sqrt(arg.t);
return Rall1d<T, V, S>(v, (0.5 / v) * arg.grad);
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> atan(const Rall1d<T, V, S> &x)
{
T v;
v = (atan(x.t));
return Rall1d<T, V, S>(v, x.grad / (1.0 + sqr(x.t)));
}
template<class T, class V, class S>
INLINE Rall1d<T, V, S> hypot(const Rall1d<T, V, S> &y, const Rall1d<T, V, S> &x)
{
T v;
v = (hypot(y.t, x.t));
return Rall1d<T, V, S>(v, (x.t / v) * x.grad + (y.t / v) * y.grad);
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> asin(const Rall1d<T, V, S> &x)
{
T v;
v = (asin(x.t));
return Rall1d<T, V, S>(v, x.grad / sqrt(1.0 - sqr(x.t)));
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> acos(const Rall1d<T, V, S> &x)
{
T v;
v = (acos(x.t));
return Rall1d<T, V, S>(v, -x.grad / sqrt(1.0 - sqr(x.t)));
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> abs(const Rall1d<T, V, S> &x)
{
T v;
v = (Sign(x));
return Rall1d<T, V, S>(v * x, v * x.grad);
}
template<class T, class V, class S> INLINE S Norm(const Rall1d<T, V, S> &value)
{
return Norm(value.t);
}
template<class T, class V, class S> INLINE Rall1d<T, V, S> tanh(const Rall1d<T, V, S> &arg)
{
T v(tanh(arg.t));
return Rall1d<T, V, S>(v, arg.grad / sqr(cosh(arg.t)));
}
template<class T, class V, class S>
INLINE Rall1d<T, V, S> atan2(const Rall1d<T, V, S> &y, const Rall1d<T, V, S> &x)
{
T v(x.t * x.t + y.t * y.t);
return Rall1d<T, V, S>(atan2(y.t, x.t), (x.t * y.grad - y.t * x.grad) / v);
}
template<class T, class V, class S>
INLINE Rall1d<T, V, S> LinComb(S alfa,
const Rall1d<T, V, S> &a,
const T &beta,
const Rall1d<T, V, S> &b)
{
return Rall1d<T, V, S>(LinComb(alfa, a.t, beta, b.t), LinComb(alfa, a.grad, beta, b.grad));
}
template<class T, class V, class S>
INLINE void LinCombR(S alfa,
const Rall1d<T, V, S> &a,
const T &beta,
const Rall1d<T, V, S> &b,
Rall1d<T, V, S> &result)
{
LinCombR(alfa, a.t, beta, b.t, result.t);
LinCombR(alfa, a.grad, beta, b.grad, result.grad);
}
template<class T, class V, class S> INLINE void SetToZero(Rall1d<T, V, S> &value)
{
SetToZero(value.grad);
SetToZero(value.t);
}
template<class T, class V, class S> INLINE void SetToIdentity(Rall1d<T, V, S> &value)
{
SetToIdentity(value.t);
SetToZero(value.grad);
}
template<class T, class V, class S>
INLINE bool Equal(const Rall1d<T, V, S> &y, const Rall1d<T, V, S> &x, double eps = epsilon)
{
return (Equal(x.t, y.t, eps) && Equal(x.grad, y.grad, eps));
}
} // namespace KDL
#endif

870
intern/itasc/kdl/utilities/rall2d.h
View File

@@ -1,518 +1,536 @@
/*****************************************************************************
* \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 "utility.h"
#include <assert.h>
#include <math.h>
#include <assert.h>
#include "utility.h"
namespace KDL {
/**
* Rall2d contains a value, and its gradient and its 2nd derivative, and defines an algebraic
* 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<double>, Rall1d<double>, double> ).
*
* S is always passed by value.
* S is always passed by value.
*
* \par Class Type
* Concrete implementation
*/
template<class T, class V = T, class S = T> class Rall2d {
public:
T t; //!< value
V d; //!< 1st derivative
V dd; //!< 2nd derivative
public:
// = Constructors
INLINE Rall2d() {}
template <class T,class V=T,class S=T>
class Rall2d
{
public :
T t; //!< value
V d; //!< 1st derivative
V dd; //!< 2nd derivative
public :
// = Constructors
INLINE Rall2d() {}
explicit INLINE Rall2d(typename TI<T>::Arg c)
{
t = c;
SetToZero(d);
SetToZero(dd);
}
explicit INLINE Rall2d(typename TI<T>::Arg c)
{t=c;SetToZero(d);SetToZero(dd);}
INLINE Rall2d(typename TI<T>::Arg tn, const V &afg) : t(tn), d(afg)
{
SetToZero(dd);
}
INLINE Rall2d(typename TI<T>::Arg tn,const V& afg):t(tn),d(afg) {SetToZero(dd);}
INLINE Rall2d(typename TI<T>::Arg tn, const V &afg, const V &afg2) : t(tn), d(afg), dd(afg2) {}
INLINE Rall2d(typename TI<T>::Arg tn,const V& afg,const V& afg2):t(tn),d(afg),dd(afg2) {}
// = Copy Constructor
INLINE Rall2d(const Rall2d<T, V, S> &r) : t(r.t), d(r.d), dd(r.dd) {}
// if one defines this constructor, it's better optimized then the
// automatically generated one ( that one set's up a loop to copy
// word by word.
// = Copy Constructor
INLINE Rall2d(const Rall2d<T,V,S>& r):t(r.t),d(r.d),dd(r.dd) {}
//if one defines this constructor, it's better optimized then the
//automatically generated one ( that one set's up a loop to copy
// word by word.
// = Member functions to access internal structures :
INLINE T& Value() {
return t;
}
// = Member functions to access internal structures :
INLINE T &Value()
{
return t;
}
INLINE V& D() {
return d;
}
INLINE V &D()
{
return d;
}
INLINE V& DD() {
return dd;
}
INLINE static Rall2d<T,V,S> Zero() {
Rall2d<T,V,S> tmp;
SetToZero(tmp);
return tmp;
}
INLINE static Rall2d<T,V,S> Identity() {
Rall2d<T,V,S> tmp;
SetToIdentity(tmp);
return tmp;
}
INLINE V &DD()
{
return dd;
}
INLINE static Rall2d<T, V, S> Zero()
{
Rall2d<T, V, S> tmp;
SetToZero(tmp);
return tmp;
}
INLINE static Rall2d<T, V, S> Identity()
{
Rall2d<T, V, S> tmp;
SetToIdentity(tmp);
return tmp;
}
// = assignment operators
INLINE Rall2d<T,V,S>& operator =(S c)
{t=c;SetToZero(d);SetToZero(dd);return *this;}
// = assignment operators
INLINE Rall2d<T, V, S> &operator=(S c)
{
t = c;
SetToZero(d);
SetToZero(dd);
return *this;
}
INLINE Rall2d<T,V,S>& operator =(const Rall2d<T,V,S>& r)
{t=r.t;d=r.d;dd=r.dd;return *this;}
INLINE Rall2d<T, V, S> &operator=(const Rall2d<T, V, S> &r)
{
t = r.t;
d = r.d;
dd = r.dd;
return *this;
}
INLINE Rall2d<T,V,S>& operator /=(const Rall2d<T,V,S>& rhs)
{
t /= rhs.t;
d = (d-t*rhs.d)/rhs.t;
dd= (dd - S(2)*d*rhs.d-t*rhs.dd)/rhs.t;
return *this;
}
INLINE Rall2d<T, V, S> &operator/=(const Rall2d<T, V, S> &rhs)
{
t /= rhs.t;
d = (d - t * rhs.d) / rhs.t;
dd = (dd - S(2) * d * rhs.d - t * rhs.dd) / rhs.t;
return *this;
}
INLINE Rall2d<T,V,S>& operator *=(const Rall2d<T,V,S>& rhs)
{
t *= rhs.t;
d = (d*rhs.t+t*rhs.d);
dd = (dd*rhs.t+S(2)*d*rhs.d+t*rhs.dd);
return *this;
}
INLINE Rall2d<T, V, S> &operator*=(const Rall2d<T, V, S> &rhs)
{
t *= rhs.t;
d = (d * rhs.t + t * rhs.d);
dd = (dd * rhs.t + S(2) * d * rhs.d + t * rhs.dd);
return *this;
}
INLINE Rall2d<T,V,S>& operator +=(const Rall2d<T,V,S>& rhs)
{
t +=rhs.t;
d +=rhs.d;
dd+=rhs.dd;
return *this;
}
INLINE Rall2d<T, V, S> &operator+=(const Rall2d<T, V, S> &rhs)
{
t += rhs.t;
d += rhs.d;
dd += rhs.dd;
return *this;
}
INLINE Rall2d<T,V,S>& operator -=(const Rall2d<T,V,S>& rhs)
{
t -= rhs.t;
d -= rhs.d;
dd -= rhs.dd;
return *this;
}
INLINE Rall2d<T, V, S> &operator-=(const Rall2d<T, V, S> &rhs)
{
t -= rhs.t;
d -= rhs.d;
dd -= rhs.dd;
return *this;
}
INLINE Rall2d<T,V,S>& operator /=(S rhs)
{
t /= rhs;
d /= rhs;
dd /= rhs;
return *this;
}
INLINE Rall2d<T, V, S> &operator/=(S rhs)
{
t /= rhs;
d /= rhs;
dd /= rhs;
return *this;
}
INLINE Rall2d<T,V,S>& operator *=(S rhs)
{
t *= rhs;
d *= rhs;
dd *= rhs;
return *this;
}
INLINE Rall2d<T, V, S> &operator*=(S rhs)
{
t *= rhs;
d *= rhs;
dd *= rhs;
return *this;
}
INLINE Rall2d<T,V,S>& operator -=(S rhs)
{
t -= rhs;
return *this;
}
INLINE Rall2d<T, V, S> &operator-=(S rhs)
{
t -= rhs;
return *this;
}
INLINE Rall2d<T,V,S>& operator +=(S rhs)
{
t += rhs;
return *this;
}
INLINE Rall2d<T, V, S> &operator+=(S rhs)
{
t += rhs;
return *this;
}
// = Operators between Rall2d objects
/*
friend INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
friend INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
friend INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> operator *(S s,const Rall2d<T,V,S>& v);
friend INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& v,S s);
friend INLINE Rall2d<T,V,S> operator +(S s,const Rall2d<T,V,S>& v);
friend INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& v,S s);
friend INLINE Rall2d<T,V,S> operator -(S s,const Rall2d<T,V,S>& v);
friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& v,S s);
friend INLINE Rall2d<T,V,S> operator /(S s,const Rall2d<T,V,S>& v);
friend INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& v,S s);
// = Mathematical functions that operate on Rall2d objects
friend INLINE Rall2d<T,V,S> exp(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> log(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> sin(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> cos(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> tan(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> sinh(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> cosh(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> tanh(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> sqr(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> pow(const Rall2d<T,V,S>& arg,double m) ;
friend INLINE Rall2d<T,V,S> sqrt(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> asin(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> acos(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> atan(const Rall2d<T,V,S>& x);
friend INLINE Rall2d<T,V,S> atan2(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x);
friend INLINE Rall2d<T,V,S> abs(const Rall2d<T,V,S>& x);
friend INLINE Rall2d<T,V,S> hypot(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x);
// returns sqrt(y*y+x*x), but is optimized for accuracy and speed.
friend INLINE S Norm(const Rall2d<T,V,S>& value) ;
// returns Norm( value.Value() ).
// = Some utility functions to improve performance
// (should also be declared on primitive types to improve uniformity
friend INLINE Rall2d<T,V,S> LinComb(S alfa,const Rall2d<T,V,S>& a,
TI<T>::Arg beta,const Rall2d<T,V,S>& b );
friend INLINE void LinCombR(S alfa,const Rall2d<T,V,S>& a,
TI<T>::Arg beta,const Rall2d<T,V,S>& b,Rall2d<T,V,S>& result );
// = Setting value of a Rall2d object to 0 or 1
friend INLINE void SetToZero(Rall2d<T,V,S>& value);
friend INLINE void SetToOne(Rall2d<T,V,S>& value);
// = Equality in an eps-interval
friend INLINE bool Equal(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x,double eps);
*/
};
// = Operators between Rall2d objects
/*
friend INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>&
rhs); friend INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>&
rhs); friend INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>&
rhs); friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>&
rhs); friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& arg); friend INLINE
Rall2d<T,V,S> operator *(S s,const Rall2d<T,V,S>& v); friend INLINE Rall2d<T,V,S> operator
*(const Rall2d<T,V,S>& v,S s); friend INLINE Rall2d<T,V,S> operator +(S s,const Rall2d<T,V,S>&
v); friend INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& v,S s); friend INLINE
Rall2d<T,V,S> operator -(S s,const Rall2d<T,V,S>& v); friend INLINE Rall2d<T,V,S> operator
-(const Rall2d<T,V,S>& v,S s); friend INLINE Rall2d<T,V,S> operator /(S s,const Rall2d<T,V,S>&
v); friend INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& v,S s);
// = Mathematical functions that operate on Rall2d objects
friend INLINE Rall2d<T,V,S> exp(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> log(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> sin(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> cos(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> tan(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> sinh(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> cosh(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> tanh(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> sqr(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> pow(const Rall2d<T,V,S>& arg,double m) ;
friend INLINE Rall2d<T,V,S> sqrt(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> asin(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> acos(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> atan(const Rall2d<T,V,S>& x);
friend INLINE Rall2d<T,V,S> atan2(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x);
friend INLINE Rall2d<T,V,S> abs(const Rall2d<T,V,S>& x);
friend INLINE Rall2d<T,V,S> hypot(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x);
// returns sqrt(y*y+x*x), but is optimized for accuracy and speed.
friend INLINE S Norm(const Rall2d<T,V,S>& value) ;
// returns Norm( value.Value() ).
// = Some utility functions to improve performance
// (should also be declared on primitive types to improve uniformity
friend INLINE Rall2d<T,V,S> LinComb(S alfa,const Rall2d<T,V,S>& a,
TI<T>::Arg beta,const Rall2d<T,V,S>& b );
friend INLINE void LinCombR(S alfa,const Rall2d<T,V,S>& a,
TI<T>::Arg beta,const Rall2d<T,V,S>& b,Rall2d<T,V,S>& result );
// = Setting value of a Rall2d object to 0 or 1
friend INLINE void SetToZero(Rall2d<T,V,S>& value);
friend INLINE void SetToOne(Rall2d<T,V,S>& value);
// = Equality in an eps-interval
friend INLINE bool Equal(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x,double eps);
*/
};
// = Operators between Rall2d objects
template<class T, class V, class S>
INLINE Rall2d<T, V, S> operator/(const Rall2d<T, V, S> &lhs, const Rall2d<T, V, S> &rhs)
{
Rall2d<T, V, S> tmp;
tmp.t = lhs.t / rhs.t;
tmp.d = (lhs.d - tmp.t * rhs.d) / rhs.t;
tmp.dd = (lhs.dd - S(2) * tmp.d * rhs.d - tmp.t * rhs.dd) / rhs.t;
return tmp;
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
{
Rall2d<T,V,S> tmp;
tmp.t = lhs.t/rhs.t;
tmp.d = (lhs.d-tmp.t*rhs.d)/rhs.t;
tmp.dd= (lhs.dd-S(2)*tmp.d*rhs.d-tmp.t*rhs.dd)/rhs.t;
return tmp;
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
{
Rall2d<T,V,S> tmp;
tmp.t = lhs.t*rhs.t;
tmp.d = (lhs.d*rhs.t+lhs.t*rhs.d);
tmp.dd = (lhs.dd*rhs.t+S(2)*lhs.d*rhs.d+lhs.t*rhs.dd);
return tmp;
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
{
return Rall2d<T,V,S>(lhs.t+rhs.t,lhs.d+rhs.d,lhs.dd+rhs.dd);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
{
return Rall2d<T,V,S>(lhs.t-rhs.t,lhs.d-rhs.d,lhs.dd-rhs.dd);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& arg)
{
return Rall2d<T,V,S>(-arg.t,-arg.d,-arg.dd);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator *(S s,const Rall2d<T,V,S>& v)
{
return Rall2d<T,V,S>(s*v.t,s*v.d,s*v.dd);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& v,S s)
{
return Rall2d<T,V,S>(v.t*s,v.d*s,v.dd*s);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator +(S s,const Rall2d<T,V,S>& v)
{
return Rall2d<T,V,S>(s+v.t,v.d,v.dd);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& v,S s)
{
return Rall2d<T,V,S>(v.t+s,v.d,v.dd);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator -(S s,const Rall2d<T,V,S>& v)
{
return Rall2d<T,V,S>(s-v.t,-v.d,-v.dd);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& v,S s)
{
return Rall2d<T,V,S>(v.t-s,v.d,v.dd);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator /(S s,const Rall2d<T,V,S>& rhs)
{
Rall2d<T,V,S> tmp;
tmp.t = s/rhs.t;
tmp.d = (-tmp.t*rhs.d)/rhs.t;
tmp.dd= (-S(2)*tmp.d*rhs.d-tmp.t*rhs.dd)/rhs.t;
return tmp;
}
template<class T, class V, class S>
INLINE Rall2d<T, V, S> operator*(const Rall2d<T, V, S> &lhs, const Rall2d<T, V, S> &rhs)
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& v,S s)
{
return Rall2d<T,V,S>(v.t/s,v.d/s,v.dd/s);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> exp(const Rall2d<T,V,S>& arg)
{
Rall2d<T,V,S> tmp;
tmp.t = exp(arg.t);
tmp.d = tmp.t*arg.d;
tmp.dd = tmp.d*arg.d+tmp.t*arg.dd;
return tmp;
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> log(const Rall2d<T,V,S>& arg)
{
Rall2d<T,V,S> tmp;
tmp.t = log(arg.t);
tmp.d = arg.d/arg.t;
tmp.dd = (arg.dd-tmp.d*arg.d)/arg.t;
return tmp;
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> sin(const Rall2d<T,V,S>& arg)
{
T v1 = sin(arg.t);
T v2 = cos(arg.t);
return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd - (v1*arg.d)*arg.d );
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> cos(const Rall2d<T,V,S>& arg)
{
T v1 = cos(arg.t);
T v2 = -sin(arg.t);
return Rall2d<T,V,S>(v1,v2*arg.d, v2*arg.dd - (v1*arg.d)*arg.d);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> tan(const Rall2d<T,V,S>& arg)
{
T v1 = tan(arg.t);
T v2 = S(1)+sqr(v1);
return Rall2d<T,V,S>(v1,v2*arg.d, v2*(arg.dd+(S(2)*v1*sqr(arg.d))));
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> sinh(const Rall2d<T,V,S>& arg)
{
T v1 = sinh(arg.t);
T v2 = cosh(arg.t);
return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd + (v1*arg.d)*arg.d );
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> cosh(const Rall2d<T,V,S>& arg)
{
T v1 = cosh(arg.t);
T v2 = sinh(arg.t);
return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd + (v1*arg.d)*arg.d );
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> tanh(const Rall2d<T,V,S>& arg)
{
T v1 = tanh(arg.t);
T v2 = S(1)-sqr(v1);
return Rall2d<T,V,S>(v1,v2*arg.d, v2*(arg.dd-(S(2)*v1*sqr(arg.d))));
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> sqr(const Rall2d<T,V,S>& arg)
{
return Rall2d<T,V,S>(arg.t*arg.t,
(S(2)*arg.t)*arg.d,
S(2)*(sqr(arg.d)+arg.t*arg.dd)
);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> pow(const Rall2d<T,V,S>& arg,double m)
{
Rall2d<T,V,S> tmp;
tmp.t = pow(arg.t,m);
T v2 = (m/arg.t)*tmp.t;
tmp.d = v2*arg.d;
tmp.dd = (S((m-1))/arg.t)*tmp.d*arg.d + v2*arg.dd;
return tmp;
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> sqrt(const Rall2d<T,V,S>& arg)
{
/* By inversion of sqr(x) :*/
Rall2d<T,V,S> tmp;
tmp.t = sqrt(arg.t);
tmp.d = (S(0.5)/tmp.t)*arg.d;
tmp.dd = (S(0.5)*arg.dd-sqr(tmp.d))/tmp.t;
return tmp;
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> asin(const Rall2d<T,V,S>& arg)
{
Rall2d<T, V, S> tmp;
tmp.t = lhs.t * rhs.t;
tmp.d = (lhs.d * rhs.t + lhs.t * rhs.d);
tmp.dd = (lhs.dd * rhs.t + S(2) * lhs.d * rhs.d + lhs.t * rhs.dd);
return tmp;
/* By inversion of sin(x) */
Rall2d<T,V,S> tmp;
tmp.t = asin(arg.t);
T v = cos(tmp.t);
tmp.d = arg.d/v;
tmp.dd = (arg.dd+arg.t*sqr(tmp.d))/v;
return tmp;
}
template<class T, class V, class S>
INLINE Rall2d<T, V, S> operator+(const Rall2d<T, V, S> &lhs, const Rall2d<T, V, S> &rhs)
template <class T,class V,class S>
INLINE Rall2d<T,V,S> acos(const Rall2d<T,V,S>& arg)
{
return Rall2d<T, V, S>(lhs.t + rhs.t, lhs.d + rhs.d, lhs.dd + rhs.dd);
/* By inversion of cos(x) */
Rall2d<T,V,S> tmp;
tmp.t = acos(arg.t);
T v = -sin(tmp.t);
tmp.d = arg.d/v;
tmp.dd = (arg.dd+arg.t*sqr(tmp.d))/v;
return tmp;
}
template<class T, class V, class S>
INLINE Rall2d<T, V, S> operator-(const Rall2d<T, V, S> &lhs, const Rall2d<T, V, S> &rhs)
template <class T,class V,class S>
INLINE Rall2d<T,V,S> atan(const Rall2d<T,V,S>& x)
{
return Rall2d<T, V, S>(lhs.t - rhs.t, lhs.d - rhs.d, lhs.dd - rhs.dd);
/* By inversion of tan(x) */
Rall2d<T,V,S> tmp;
tmp.t = atan(x.t);
T v = S(1)+sqr(x.t);
tmp.d = x.d/v;
tmp.dd = x.dd/v-(S(2)*x.t)*sqr(tmp.d);
return tmp;
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> operator-(const Rall2d<T, V, S> &arg)
template <class T,class V,class S>
INLINE Rall2d<T,V,S> atan2(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x)
{
return Rall2d<T, V, S>(-arg.t, -arg.d, -arg.dd);
Rall2d<T,V,S> tmp;
tmp.t = atan2(y.t,x.t);
T v = sqr(y.t)+sqr(x.t);
tmp.d = (x.t*y.d-x.d*y.t)/v;
tmp.dd = ( x.t*y.dd-x.dd*y.t-S(2)*(x.t*x.d+y.t*y.d)*tmp.d ) / v;
return tmp;
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> operator*(S s, const Rall2d<T, V, S> &v)
template <class T,class V,class S>
INLINE Rall2d<T,V,S> abs(const Rall2d<T,V,S>& x)
{
return Rall2d<T, V, S>(s * v.t, s * v.d, s * v.dd);
T v(Sign(x));
return Rall2d<T,V,S>(v*x,v*x.d,v*x.dd);
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> operator*(const Rall2d<T, V, S> &v, S s)
template <class T,class V,class S>
INLINE Rall2d<T,V,S> hypot(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x)
{
return Rall2d<T, V, S>(v.t * s, v.d * s, v.dd * s);
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> operator+(S s, const Rall2d<T, V, S> &v)
{
return Rall2d<T, V, S>(s + v.t, v.d, v.dd);
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> operator+(const Rall2d<T, V, S> &v, S s)
{
return Rall2d<T, V, S>(v.t + s, v.d, v.dd);
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> operator-(S s, const Rall2d<T, V, S> &v)
{
return Rall2d<T, V, S>(s - v.t, -v.d, -v.dd);
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> operator-(const Rall2d<T, V, S> &v, S s)
{
return Rall2d<T, V, S>(v.t - s, v.d, v.dd);
}
template<class T, class V, class S>
INLINE Rall2d<T, V, S> operator/(S s, const Rall2d<T, V, S> &rhs)
{
Rall2d<T, V, S> tmp;
tmp.t = s / rhs.t;
tmp.d = (-tmp.t * rhs.d) / rhs.t;
tmp.dd = (-S(2) * tmp.d * rhs.d - tmp.t * rhs.dd) / rhs.t;
return tmp;
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> operator/(const Rall2d<T, V, S> &v, S s)
{
return Rall2d<T, V, S>(v.t / s, v.d / s, v.dd / s);
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> exp(const Rall2d<T, V, S> &arg)
{
Rall2d<T, V, S> tmp;
tmp.t = exp(arg.t);
tmp.d = tmp.t * arg.d;
tmp.dd = tmp.d * arg.d + tmp.t * arg.dd;
return tmp;
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> log(const Rall2d<T, V, S> &arg)
{
Rall2d<T, V, S> tmp;
tmp.t = log(arg.t);
tmp.d = arg.d / arg.t;
tmp.dd = (arg.dd - tmp.d * arg.d) / arg.t;
return tmp;
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> sin(const Rall2d<T, V, S> &arg)
{
T v1 = sin(arg.t);
T v2 = cos(arg.t);
return Rall2d<T, V, S>(v1, v2 * arg.d, v2 * arg.dd - (v1 * arg.d) * arg.d);
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> cos(const Rall2d<T, V, S> &arg)
{
T v1 = cos(arg.t);
T v2 = -sin(arg.t);
return Rall2d<T, V, S>(v1, v2 * arg.d, v2 * arg.dd - (v1 * arg.d) * arg.d);
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> tan(const Rall2d<T, V, S> &arg)
{
T v1 = tan(arg.t);
T v2 = S(1) + sqr(v1);
return Rall2d<T, V, S>(v1, v2 * arg.d, v2 * (arg.dd + (S(2) * v1 * sqr(arg.d))));
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> sinh(const Rall2d<T, V, S> &arg)
{
T v1 = sinh(arg.t);
T v2 = cosh(arg.t);
return Rall2d<T, V, S>(v1, v2 * arg.d, v2 * arg.dd + (v1 * arg.d) * arg.d);
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> cosh(const Rall2d<T, V, S> &arg)
{
T v1 = cosh(arg.t);
T v2 = sinh(arg.t);
return Rall2d<T, V, S>(v1, v2 * arg.d, v2 * arg.dd + (v1 * arg.d) * arg.d);
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> tanh(const Rall2d<T, V, S> &arg)
{
T v1 = tanh(arg.t);
T v2 = S(1) - sqr(v1);
return Rall2d<T, V, S>(v1, v2 * arg.d, v2 * (arg.dd - (S(2) * v1 * sqr(arg.d))));
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> sqr(const Rall2d<T, V, S> &arg)
{
return Rall2d<T, V, S>(
arg.t * arg.t, (S(2) * arg.t) * arg.d, S(2) * (sqr(arg.d) + arg.t * arg.dd));
}
template<class T, class V, class S>
INLINE Rall2d<T, V, S> pow(const Rall2d<T, V, S> &arg, double m)
{
Rall2d<T, V, S> tmp;
tmp.t = pow(arg.t, m);
T v2 = (m / arg.t) * tmp.t;
tmp.d = v2 * arg.d;
tmp.dd = (S((m - 1)) / arg.t) * tmp.d * arg.d + v2 * arg.dd;
return tmp;
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> sqrt(const Rall2d<T, V, S> &arg)
{
/* By inversion of sqr(x) :*/
Rall2d<T, V, S> tmp;
tmp.t = sqrt(arg.t);
tmp.d = (S(0.5) / tmp.t) * arg.d;
tmp.dd = (S(0.5) * arg.dd - sqr(tmp.d)) / tmp.t;
return tmp;
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> asin(const Rall2d<T, V, S> &arg)
{
/* By inversion of sin(x) */
Rall2d<T, V, S> tmp;
tmp.t = asin(arg.t);
T v = cos(tmp.t);
tmp.d = arg.d / v;
tmp.dd = (arg.dd + arg.t * sqr(tmp.d)) / v;
return tmp;
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> acos(const Rall2d<T, V, S> &arg)
{
/* By inversion of cos(x) */
Rall2d<T, V, S> tmp;
tmp.t = acos(arg.t);
T v = -sin(tmp.t);
tmp.d = arg.d / v;
tmp.dd = (arg.dd + arg.t * sqr(tmp.d)) / v;
return tmp;
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> atan(const Rall2d<T, V, S> &x)
{
/* By inversion of tan(x) */
Rall2d<T, V, S> tmp;
tmp.t = atan(x.t);
T v = S(1) + sqr(x.t);
tmp.d = x.d / v;
tmp.dd = x.dd / v - (S(2) * x.t) * sqr(tmp.d);
return tmp;
}
template<class T, class V, class S>
INLINE Rall2d<T, V, S> atan2(const Rall2d<T, V, S> &y, const Rall2d<T, V, S> &x)
{
Rall2d<T, V, S> tmp;
tmp.t = atan2(y.t, x.t);
T v = sqr(y.t) + sqr(x.t);
tmp.d = (x.t * y.d - x.d * y.t) / v;
tmp.dd = (x.t * y.dd - x.dd * y.t - S(2) * (x.t * x.d + y.t * y.d) * tmp.d) / v;
return tmp;
}
template<class T, class V, class S> INLINE Rall2d<T, V, S> abs(const Rall2d<T, V, S> &x)
{
T v(Sign(x));
return Rall2d<T, V, S>(v * x, v * x.d, v * x.dd);
}
template<class T, class V, class S>
INLINE Rall2d<T, V, S> hypot(const Rall2d<T, V, S> &y, const Rall2d<T, V, S> &x)
{
Rall2d<T, V, S> tmp;
tmp.t = hypot(y.t, x.t);
tmp.d = (x.t * x.d + y.t * y.d) / tmp.t;
tmp.dd = (sqr(x.d) + x.t * x.dd + sqr(y.d) + y.t * y.dd - sqr(tmp.d)) / tmp.t;
return tmp;
Rall2d<T,V,S> tmp;
tmp.t = hypot(y.t,x.t);
tmp.d = (x.t*x.d+y.t*y.d)/tmp.t;
tmp.dd = (sqr(x.d)+x.t*x.dd+sqr(y.d)+y.t*y.dd-sqr(tmp.d))/tmp.t;
return tmp;
}
// returns sqrt(y*y+x*x), but is optimized for accuracy and speed.
template<class T, class V, class S> INLINE S Norm(const Rall2d<T, V, S> &value)
template <class T,class V,class S>
INLINE S Norm(const Rall2d<T,V,S>& value)
{
return Norm(value.t);
return Norm(value.t);
}
// returns Norm( value.Value() ).
// (should also be declared on primitive types to improve uniformity
template<class T, class V, class S>
INLINE Rall2d<T, V, S> LinComb(S alfa,
const Rall2d<T, V, S> &a,
const T &beta,
const Rall2d<T, V, S> &b)
{
return Rall2d<T, V, S>(LinComb(alfa, a.t, beta, b.t),
LinComb(alfa, a.d, beta, b.d),
LinComb(alfa, a.dd, beta, b.dd));
template <class T,class V,class S>
INLINE Rall2d<T,V,S> LinComb(S alfa,const Rall2d<T,V,S>& a,
const T& beta,const Rall2d<T,V,S>& b ) {
return Rall2d<T,V,S>(
LinComb(alfa,a.t,beta,b.t),
LinComb(alfa,a.d,beta,b.d),
LinComb(alfa,a.dd,beta,b.dd)
);
}
template<class T, class V, class S>
INLINE void LinCombR(S alfa,
const Rall2d<T, V, S> &a,
const T &beta,
const Rall2d<T, V, S> &b,
Rall2d<T, V, S> &result)
{
LinCombR(alfa, a.t, beta, b.t, result.t);
LinCombR(alfa, a.d, beta, b.d, result.d);
LinCombR(alfa, a.dd, beta, b.dd, result.dd);
template <class T,class V,class S>
INLINE void LinCombR(S alfa,const Rall2d<T,V,S>& a,
const T& beta,const Rall2d<T,V,S>& b,Rall2d<T,V,S>& result ) {
LinCombR(alfa, a.t, beta, b.t, result.t);
LinCombR(alfa, a.d, beta, b.d, result.d);
LinCombR(alfa, a.dd, beta, b.dd, result.dd);
}
template<class T, class V, class S> INLINE void SetToZero(Rall2d<T, V, S> &value)
template <class T,class V,class S>
INLINE void SetToZero(Rall2d<T,V,S>& value)
{
SetToZero(value.t);
SetToZero(value.d);
SetToZero(value.dd);
}
template <class T,class V,class S>
INLINE void SetToIdentity(Rall2d<T,V,S>& value)
{
SetToZero(value.d);
SetToIdentity(value.t);
SetToZero(value.dd);
}
template <class T,class V,class S>
INLINE bool Equal(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x,double eps=epsilon)
{
SetToZero(value.t);
SetToZero(value.d);
SetToZero(value.dd);
return (Equal(x.t,y.t,eps)&&
Equal(x.d,y.d,eps)&&
Equal(x.dd,y.dd,eps)
);
}
template<class T, class V, class S> INLINE void SetToIdentity(Rall2d<T, V, S> &value)
{
SetToZero(value.d);
SetToIdentity(value.t);
SetToZero(value.dd);
}
template<class T, class V, class S>
INLINE bool Equal(const Rall2d<T, V, S> &y, const Rall2d<T, V, S> &x, double eps = epsilon)
{
return (Equal(x.t, y.t, eps) && Equal(x.d, y.d, eps) && Equal(x.dd, y.dd, eps));
}
} // namespace KDL
#endif

575
intern/itasc/kdl/utilities/svd_eigen_HH.hpp
View File

@@ -19,310 +19,291 @@
// 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 <Eigen/Core>
#include <algorithm>
namespace KDL {
template<typename Scalar> inline Scalar PYTHAG(Scalar a, Scalar b)
namespace KDL
{
double at, bt, ct;
at = fabs(a);
bt = fabs(b);
if (at > bt) {
ct = bt / at;
return Scalar(at * sqrt(1.0 + ct * ct));
}
else {
if (bt == 0)
return Scalar(0.0);
else {
ct = at / bt;
return Scalar(bt * sqrt(1.0 + ct * ct));
template<typename Scalar> inline Scalar PYTHAG(Scalar a,Scalar b) {
double at,bt,ct;
at = fabs(a);
bt = fabs(b);
if (at > bt ) {
ct=bt/at;
return Scalar(at*sqrt(1.0+ct*ct));
} else {
if (bt==0)
return Scalar(0.0);
else {
ct=at/bt;
return Scalar(bt*sqrt(1.0+ct*ct));
}
}
}
}
template<typename Scalar> inline Scalar SIGN(Scalar a,Scalar b) {
return ((b) >= Scalar(0.0) ? fabs(a) : -fabs(a));
}
/**
* svd calculation of boost ublas matrices
*
* @param A matrix<double>(mxn)
* @param U matrix<double>(mxn)
* @param S vector<double> n
* @param V matrix<double>(nxn)
* @param tmp vector<double> n
* @param maxiter defaults to 150
*
* @return -2 if maxiter exceeded, 0 otherwise
*/
template<typename MatrixA, typename MatrixUV, typename VectorS>
int svd_eigen_HH(
const Eigen::MatrixBase<MatrixA>& A,
Eigen::MatrixBase<MatrixUV>& U,
Eigen::MatrixBase<VectorS>& S,
Eigen::MatrixBase<MatrixUV>& V,
Eigen::MatrixBase<VectorS>& tmp,
int maxiter=150)
{
//get the rows/columns of the matrix
const int rows = A.rows();
const int cols = A.cols();
U = A;
int i(-1),its(-1),j(-1),jj(-1),k(-1),nm=0;
int ppi(0);
bool flag;
e_scalar maxarg1,maxarg2,anorm(0),c(0),f(0),h(0),s(0),scale(0),x(0),y(0),z(0),g(0);
g=scale=anorm=e_scalar(0.0);
/* Householder reduction to bidiagonal form. */
for (i=0;i<cols;i++) {
ppi=i+1;
tmp(i)=scale*g;
g=s=scale=e_scalar(0.0);
if (i<rows) {
// compute the sum of the i-th column, starting from the i-th row
for (k=i;k<rows;k++) scale += fabs(U(k,i));
if (scale!=0) {
// multiply the i-th column by 1.0/scale, start from the i-th element
// sum of squares of column i, start from the i-th element
for (k=i;k<rows;k++) {
U(k,i) /= scale;
s += U(k,i)*U(k,i);
}
f=U(i,i); // f is the diag elem
g = -SIGN(e_scalar(sqrt(s)),f);
h=f*g-s;
U(i,i)=f-g;
for (j=ppi;j<cols;j++) {
// dot product of columns i and j, starting from the i-th row
for (s=0.0,k=i;k<rows;k++) s += U(k,i)*U(k,j);
f=s/h;
// copy the scaled i-th column into the j-th column
for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
}
for (k=i;k<rows;k++) U(k,i) *= scale;
}
}
// save singular value
S(i)=scale*g;
g=s=scale=e_scalar(0.0);
if ((i <rows) && (i+1 != cols)) {
// sum of row i, start from columns i+1
for (k=ppi;k<cols;k++) scale += fabs(U(i,k));
if (scale!=0) {
for (k=ppi;k<cols;k++) {
U(i,k) /= scale;
s += U(i,k)*U(i,k);
}
f=U(i,ppi);
g = -SIGN(e_scalar(sqrt(s)),f);
h=f*g-s;
U(i,ppi)=f-g;
for (k=ppi;k<cols;k++) tmp(k)=U(i,k)/h;
for (j=ppi;j<rows;j++) {
for (s=0.0,k=ppi;k<cols;k++) s += U(j,k)*U(i,k);
for (k=ppi;k<cols;k++) U(j,k) += s*tmp(k);
}
for (k=ppi;k<cols;k++) U(i,k) *= scale;
}
}
maxarg1=anorm;
maxarg2=(fabs(S(i))+fabs(tmp(i)));
anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2;
}
/* Accumulation of right-hand transformations. */
for (i=cols-1;i>=0;i--) {
if (i<cols-1) {
if (g) {
for (j=ppi;j<cols;j++) V(j,i)=(U(i,j)/U(i,ppi))/g;
for (j=ppi;j<cols;j++) {
for (s=0.0,k=ppi;k<cols;k++) s += U(i,k)*V(k,j);
for (k=ppi;k<cols;k++) V(k,j) += s*V(k,i);
}
}
for (j=ppi;j<cols;j++) V(i,j)=V(j,i)=0.0;
}
V(i,i)=1.0;
g=tmp(i);
ppi=i;
}
/* Accumulation of left-hand transformations. */
for (i=cols-1<rows-1 ? cols-1:rows-1;i>=0;i--) {
ppi=i+1;
g=S(i);
for (j=ppi;j<cols;j++) U(i,j)=0.0;
if (g) {
g=e_scalar(1.0)/g;
for (j=ppi;j<cols;j++) {
for (s=0.0,k=ppi;k<rows;k++) s += U(k,i)*U(k,j);
f=(s/U(i,i))*g;
for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
}
for (j=i;j<rows;j++) U(j,i) *= g;
} else {
for (j=i;j<rows;j++) U(j,i)=0.0;
}
++U(i,i);
}
/* Diagonalization of the bidiagonal form. */
for (k=cols-1;k>=0;k--) { /* Loop over singular values. */
for (its=1;its<=maxiter;its++) { /* Loop over allowed iterations. */
flag=true;
for (ppi=k;ppi>=0;ppi--) { /* Test for splitting. */
nm=ppi-1; /* Note that tmp(1) is always zero. */
if ((fabs(tmp(ppi))+anorm) == anorm) {
flag=false;
break;
}
if ((fabs(S(nm)+anorm) == anorm)) break;
}
if (flag) {
c=e_scalar(0.0); /* Cancellation of tmp(l), if l>1: */
s=e_scalar(1.);
for (i=ppi;i<=k;i++) {
f=s*tmp(i);
tmp(i)=c*tmp(i);
if ((fabs(f)+anorm) == anorm) break;
g=S(i);
h=PYTHAG(f,g);
S(i)=h;
h=e_scalar(1.0)/h;
c=g*h;
s=(-f*h);
for (j=0;j<rows;j++) {
y=U(j,nm);
z=U(j,i);
U(j,nm)=y*c+z*s;
U(j,i)=z*c-y*s;
}
}
}
z=S(k);
if (ppi == k) { /* Convergence. */
if (z < e_scalar(0.0)) { /* Singular value is made nonnegative. */
S(k) = -z;
for (j=0;j<cols;j++) V(j,k)=-V(j,k);
}
break;
}
x=S(ppi); /* Shift from bottom 2-by-2 minor: */
nm=k-1;
y=S(nm);
g=tmp(nm);
h=tmp(k);
f=((y-z)*(y+z)+(g-h)*(g+h))/(e_scalar(2.0)*h*y);
g=PYTHAG(f,e_scalar(1.0));
f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
/* Next QR transformation: */
c=s=1.0;
for (j=ppi;j<=nm;j++) {
i=j+1;
g=tmp(i);
y=S(i);
h=s*g;
g=c*g;
z=PYTHAG(f,h);
tmp(j)=z;
c=f/z;
s=h/z;
f=x*c+g*s;
g=g*c-x*s;
h=y*s;
y=y*c;
for (jj=0;jj<cols;jj++) {
x=V(jj,j);
z=V(jj,i);
V(jj,j)=x*c+z*s;
V(jj,i)=z*c-x*s;
}
z=PYTHAG(f,h);
S(j)=z;
if (z) {
z=e_scalar(1.0)/z;
c=f*z;
s=h*z;
}
f=(c*g)+(s*y);
x=(c*y)-(s*g);
for (jj=0;jj<rows;jj++) {
y=U(jj,j);
z=U(jj,i);
U(jj,j)=y*c+z*s;
U(jj,i)=z*c-y*s;
}
}
tmp(ppi)=0.0;
tmp(k)=f;
S(k)=x;
}
}
//Sort eigen values:
for (i=0; i<cols; i++){
double S_max = S(i);
int i_max = i;
for (j=i+1; j<cols; j++){
double Sj = S(j);
if (Sj > S_max){
S_max = Sj;
i_max = j;
}
}
if (i_max != i){
/* swap eigenvalues */
e_scalar tmp = S(i);
S(i)=S(i_max);
S(i_max)=tmp;
/* swap eigenvectors */
U.col(i).swap(U.col(i_max));
V.col(i).swap(V.col(i_max));
}
}
if (its == maxiter)
return (-2);
else
return (0);
}
}
template<typename Scalar> inline Scalar SIGN(Scalar a, Scalar b)
{
return ((b) >= Scalar(0.0) ? fabs(a) : -fabs(a));
}
/**
* svd calculation of boost ublas matrices
*
* @param A matrix<double>(mxn)
* @param U matrix<double>(mxn)
* @param S vector<double> n
* @param V matrix<double>(nxn)
* @param tmp vector<double> n
* @param maxiter defaults to 150
*
* @return -2 if maxiter exceeded, 0 otherwise
*/
template<typename MatrixA, typename MatrixUV, typename VectorS>
int svd_eigen_HH(const Eigen::MatrixBase<MatrixA> &A,
Eigen::MatrixBase<MatrixUV> &U,
Eigen::MatrixBase<VectorS> &S,
Eigen::MatrixBase<MatrixUV> &V,
Eigen::MatrixBase<VectorS> &tmp,
int maxiter = 150)
{
// get the rows/columns of the matrix
const int rows = A.rows();
const int cols = A.cols();
U = A;
int i(-1), its(-1), j(-1), jj(-1), k(-1), nm = 0;
int ppi(0);
bool flag;
e_scalar maxarg1, maxarg2, anorm(0), c(0), f(0), h(0), s(0), scale(0), x(0), y(0), z(0), g(0);
g = scale = anorm = e_scalar(0.0);
/* Householder reduction to bidiagonal form. */
for (i = 0; i < cols; i++) {
ppi = i + 1;
tmp(i) = scale * g;
g = s = scale = e_scalar(0.0);
if (i < rows) {
// compute the sum of the i-th column, starting from the i-th row
for (k = i; k < rows; k++)
scale += fabs(U(k, i));
if (scale != 0) {
// multiply the i-th column by 1.0/scale, start from the i-th element
// sum of squares of column i, start from the i-th element
for (k = i; k < rows; k++) {
U(k, i) /= scale;
s += U(k, i) * U(k, i);
}
f = U(i, i); // f is the diag elem
g = -SIGN(e_scalar(sqrt(s)), f);
h = f * g - s;
U(i, i) = f - g;
for (j = ppi; j < cols; j++) {
// dot product of columns i and j, starting from the i-th row
for (s = 0.0, k = i; k < rows; k++)
s += U(k, i) * U(k, j);
f = s / h;
// copy the scaled i-th column into the j-th column
for (k = i; k < rows; k++)
U(k, j) += f * U(k, i);
}
for (k = i; k < rows; k++)
U(k, i) *= scale;
}
}
// save singular value
S(i) = scale * g;
g = s = scale = e_scalar(0.0);
if ((i < rows) && (i + 1 != cols)) {
// sum of row i, start from columns i+1
for (k = ppi; k < cols; k++)
scale += fabs(U(i, k));
if (scale != 0) {
for (k = ppi; k < cols; k++) {
U(i, k) /= scale;
s += U(i, k) * U(i, k);
}
f = U(i, ppi);
g = -SIGN(e_scalar(sqrt(s)), f);
h = f * g - s;
U(i, ppi) = f - g;
for (k = ppi; k < cols; k++)
tmp(k) = U(i, k) / h;
for (j = ppi; j < rows; j++) {
for (s = 0.0, k = ppi; k < cols; k++)
s += U(j, k) * U(i, k);
for (k = ppi; k < cols; k++)
U(j, k) += s * tmp(k);
}
for (k = ppi; k < cols; k++)
U(i, k) *= scale;
}
}
maxarg1 = anorm;
maxarg2 = (fabs(S(i)) + fabs(tmp(i)));
anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2;
}
/* Accumulation of right-hand transformations. */
for (i = cols - 1; i >= 0; i--) {
if (i < cols - 1) {
if (g) {
for (j = ppi; j < cols; j++)
V(j, i) = (U(i, j) / U(i, ppi)) / g;
for (j = ppi; j < cols; j++) {
for (s = 0.0, k = ppi; k < cols; k++)
s += U(i, k) * V(k, j);
for (k = ppi; k < cols; k++)
V(k, j) += s * V(k, i);
}
}
for (j = ppi; j < cols; j++)
V(i, j) = V(j, i) = 0.0;
}
V(i, i) = 1.0;
g = tmp(i);
ppi = i;
}
/* Accumulation of left-hand transformations. */
for (i = cols - 1 < rows - 1 ? cols - 1 : rows - 1; i >= 0; i--) {
ppi = i + 1;
g = S(i);
for (j = ppi; j < cols; j++)
U(i, j) = 0.0;
if (g) {
g = e_scalar(1.0) / g;
for (j = ppi; j < cols; j++) {
for (s = 0.0, k = ppi; k < rows; k++)
s += U(k, i) * U(k, j);
f = (s / U(i, i)) * g;
for (k = i; k < rows; k++)
U(k, j) += f * U(k, i);
}
for (j = i; j < rows; j++)
U(j, i) *= g;
}
else {
for (j = i; j < rows; j++)
U(j, i) = 0.0;
}
++U(i, i);
}
/* Diagonalization of the bidiagonal form. */
for (k = cols - 1; k >= 0; k--) { /* Loop over singular values. */
for (its = 1; its <= maxiter; its++) { /* Loop over allowed iterations. */
flag = true;
for (ppi = k; ppi >= 0; ppi--) { /* Test for splitting. */
nm = ppi - 1; /* Note that tmp(1) is always zero. */
if ((fabs(tmp(ppi)) + anorm) == anorm) {
flag = false;
break;
}
if ((fabs(S(nm) + anorm) == anorm))
break;
}
if (flag) {
c = e_scalar(0.0); /* Cancellation of tmp(l), if l>1: */
s = e_scalar(1.);
for (i = ppi; i <= k; i++) {
f = s * tmp(i);
tmp(i) = c * tmp(i);
if ((fabs(f) + anorm) == anorm)
break;
g = S(i);
h = PYTHAG(f, g);
S(i) = h;
h = e_scalar(1.0) / h;
c = g * h;
s = (-f * h);
for (j = 0; j < rows; j++) {
y = U(j, nm);
z = U(j, i);
U(j, nm) = y * c + z * s;
U(j, i) = z * c - y * s;
}
}
}
z = S(k);
if (ppi == k) { /* Convergence. */
if (z < e_scalar(0.0)) { /* Singular value is made nonnegative. */
S(k) = -z;
for (j = 0; j < cols; j++)
V(j, k) = -V(j, k);
}
break;
}
x = S(ppi); /* Shift from bottom 2-by-2 minor: */
nm = k - 1;
y = S(nm);
g = tmp(nm);
h = tmp(k);
f = ((y - z) * (y + z) + (g - h) * (g + h)) / (e_scalar(2.0) * h * y);
g = PYTHAG(f, e_scalar(1.0));
f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x;
/* Next QR transformation: */
c = s = 1.0;
for (j = ppi; j <= nm; j++) {
i = j + 1;
g = tmp(i);
y = S(i);
h = s * g;
g = c * g;
z = PYTHAG(f, h);
tmp(j) = z;
c = f / z;
s = h / z;
f = x * c + g * s;
g = g * c - x * s;
h = y * s;
y = y * c;
for (jj = 0; jj < cols; jj++) {
x = V(jj, j);
z = V(jj, i);
V(jj, j) = x * c + z * s;
V(jj, i) = z * c - x * s;
}
z = PYTHAG(f, h);
S(j) = z;
if (z) {
z = e_scalar(1.0) / z;
c = f * z;
s = h * z;
}
f = (c * g) + (s * y);
x = (c * y) - (s * g);
for (jj = 0; jj < rows; jj++) {
y = U(jj, j);
z = U(jj, i);
U(jj, j) = y * c + z * s;
U(jj, i) = z * c - y * s;
}
}
tmp(ppi) = 0.0;
tmp(k) = f;
S(k) = x;
}
}
// Sort eigen values:
for (i = 0; i < cols; i++) {
double S_max = S(i);
int i_max = i;
for (j = i + 1; j < cols; j++) {
double Sj = S(j);
if (Sj > S_max) {
S_max = Sj;
i_max = j;
}
}
if (i_max != i) {
/* swap eigenvalues */
e_scalar tmp = S(i);
S(i) = S(i_max);
S(i_max) = tmp;
/* swap eigenvectors */
U.col(i).swap(U.col(i_max));
V.col(i).swap(V.col(i_max));
}
}
if (its == maxiter)
return (-2);
else
return (0);
}
} // namespace KDL
#endif

128
intern/itasc/kdl/utilities/traits.h
View File

@@ -1,98 +1,114 @@
/** \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;
} // namespace KDL
class Frame;
class Rotation;
class Vector;
class Twist;
class Wrench;
class FrameVel;
class RotationVel;
class VectorVel;
class TwistVel;
}
/**
* @brief Traits are traits classes to determine the type of a derivative of another type.
*
* For geometric objects the "geometric" derivative is chosen. For example the derivative of a
Rotation
* matrix is NOT a 3x3 matrix containing the derivative of the elements of a rotation matrix. The
derivative
* of the rotation matrix is a Vector corresponding the rotational velocity. Mostly used in
template classes
* 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<Rotation> dR;
\endverbatim
*/
template<typename T> struct Traits {
typedef T valueType;
typedef T derivType;
template <typename T>
struct Traits {
typedef T valueType;
typedef T derivType;
};
template<> struct Traits<KDL::Frame> {
typedef KDL::Frame valueType;
typedef KDL::Twist derivType;
template <>
struct Traits<KDL::Frame> {
typedef KDL::Frame valueType;
typedef KDL::Twist derivType;
};
template<> struct Traits<KDL::Twist> {
typedef KDL::Twist valueType;
typedef KDL::Twist derivType;
template <>
struct Traits<KDL::Twist> {
typedef KDL::Twist valueType;
typedef KDL::Twist derivType;
};
template<> struct Traits<KDL::Wrench> {
typedef KDL::Wrench valueType;
typedef KDL::Wrench derivType;
template <>
struct Traits<KDL::Wrench> {
typedef KDL::Wrench valueType;
typedef KDL::Wrench derivType;
};
template<> struct Traits<KDL::Rotation> {
typedef KDL::Rotation valueType;
typedef KDL::Vector derivType;
template <>
struct Traits<KDL::Rotation> {
typedef KDL::Rotation valueType;
typedef KDL::Vector derivType;
};
template<> struct Traits<KDL::Vector> {
typedef KDL::Vector valueType;
typedef KDL::Vector derivType;
template <>
struct Traits<KDL::Vector> {
typedef KDL::Vector valueType;
typedef KDL::Vector derivType;
};
template<> struct Traits<double> {
typedef double valueType;
typedef double derivType;
template <>
struct Traits<double> {
typedef double valueType;
typedef double derivType;
};
template<> struct Traits<float> {
typedef float valueType;
typedef float derivType;
template <>
struct Traits<float> {
typedef float valueType;
typedef float derivType;
};
template<> struct Traits<KDL::FrameVel> {
typedef KDL::Frame valueType;
typedef KDL::TwistVel derivType;
template <>
struct Traits<KDL::FrameVel> {
typedef KDL::Frame valueType;
typedef KDL::TwistVel derivType;
};
template<> struct Traits<KDL::TwistVel> {
typedef KDL::Twist valueType;
typedef KDL::TwistVel derivType;
template <>
struct Traits<KDL::TwistVel> {
typedef KDL::Twist valueType;
typedef KDL::TwistVel derivType;
};
template<> struct Traits<KDL::RotationVel> {
typedef KDL::Rotation valueType;
typedef KDL::VectorVel derivType;
template <>
struct Traits<KDL::RotationVel> {
typedef KDL::Rotation valueType;
typedef KDL::VectorVel derivType;
};
template<> struct Traits<KDL::VectorVel> {
typedef KDL::Vector valueType;
typedef KDL::VectorVel derivType;
template <>
struct Traits<KDL::VectorVel> {
typedef KDL::Vector valueType;
typedef KDL::VectorVel derivType;
};
#endif

14
intern/itasc/kdl/utilities/utility.cpp
View File

@@ -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;
} // namespace KDL
double epsilon2 = 0.000001*0.000001;
}

360
intern/itasc/kdl/utilities/utility.h
View File

@@ -1,166 +1,170 @@
/*****************************************************************************
/*****************************************************************************
* 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 <cstdlib>
#include <cassert>
#include <cmath>
#include <cstdlib>
#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 ::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;
// so that sin,cos can be overloaded and complete
// resolution of overloaded functions work.
using ::sin;
using ::cos;
using ::exp;
using ::log;
using ::sin;
using ::cos;
using ::tan;
using ::sinh;
using ::cosh;
using ::pow;
using ::sqrt;
using ::atan;
using ::hypot;
using ::asin;
using ::acos;
using ::tanh;
using ::atan2;
#endif
#ifndef __GNUC__
// only real solution : get Rall1d and varia out of namespaces.
# pragma warning(disable : 4786)
//only real solution : get Rall1d and varia out of namespaces.
#pragma warning (disable:4786)
inline double sin(double a)
{
return ::sin(a);
}
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 cos(double a)
{
return ::cos(a);
}
inline double exp(double a)
{
return ::exp(a);
}
inline double log(double a)
{
return ::log(a);
}
inline double tan(double a)
{
return ::tan(a);
}
inline double cosh(double a)
{
return ::cosh(a);
}
inline double sinh(double a)
{
return ::sinh(a);
}
inline double sqrt(double a)
{
return ::sqrt(a);
}
inline double atan(double a)
{
return ::atan(a);
}
inline double acos(double a)
{
return ::acos(a);
}
inline double asin(double a)
{
return ::asin(a);
}
inline double tanh(double a)
{
return ::tanh(a);
}
inline double pow(double a, double b)
{
return ::pow(a, b);
}
inline double atan2(double a, double b)
{
return ::atan2(a, b);
}
#endif
/**
/**
* Auxiliary class for argument types (Trait-template class )
*
*
* Is used to pass doubles by value, and arbitrary objects by const reference.
* This is TWICE as fast (2 x less memory access) and avoids bugs in VC6++ concerning
* the assignment of the result of intrinsic functions to const double&-typed variables,
* and optimization on.
*/
template<class T> class TI {
public:
typedef const T &Arg; //!< Arg is used for passing the element to a function.
template <class T>
class TI
{
public:
typedef const T& Arg; //!< Arg is used for passing the element to a function.
};
template<> class TI<double> {
public:
typedef double Arg;
template <>
class TI<double> {
public:
typedef double Arg;
};
template<> class TI<int> {
public:
typedef int Arg;
template <>
class TI<int> {
public:
typedef int Arg;
};
/**
/**
* /note linkage
* Something fishy about the difference between C++ and C
* in C++ const values default to INTERNAL linkage, in C they default
@@ -168,11 +172,11 @@ template<> class TI<int> {
* 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;
@@ -184,126 +188,114 @@ 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 (b < a)
return a;
else
return b;
#undef max
inline double max(double a,double b) {
if (b<a)
return a;
else
return b;
}
# undef min
inline double min(double a, double b)
{
if (b < a)
return b;
else
return a;
#undef min
inline double min(double a,double b) {
if (b<a)
return b;
else
return a;
}
#endif
#ifdef _MSC_VER
// #pragma inline_depth( 255 )
// #pragma inline_recursion( on )
# define INLINE __forceinline
// #define INLINE inline
//#pragma inline_depth( 255 )
//#pragma inline_recursion( on )
#define INLINE __forceinline
//#define INLINE inline
#else
# define INLINE inline
#define INLINE inline
#endif
inline double LinComb(double alfa, double a, double beta, double b)
{
return alfa * a + beta * b;
inline double LinComb(double alfa,double a,
double beta,double b ) {
return alfa*a+beta*b;
}
inline void LinCombR(double alfa, double a, double beta, double b, double &result)
{
result = alfa * a + beta * b;
}
inline void LinCombR(double alfa,double a,
double beta,double b,double& result ) {
result=alfa*a+beta*b;
}
//! to uniformly set double, RNDouble,Vector,... objects to zero in template-classes
inline void SetToZero(double &arg)
{
arg = 0;
inline void SetToZero(double& arg) {
arg=0;
}
//! to uniformly set double, RNDouble,Vector,... objects to the identity element in
//! template-classes
inline void SetToIdentity(double &arg)
{
arg = 1;
//! to uniformly set double, RNDouble,Vector,... objects to the identity element in template-classes
inline void SetToIdentity(double& arg) {
arg=1;
}
inline double sign(double arg)
{
return (arg < 0) ? (-1) : (1);
inline double sign(double arg) {
return (arg<0)?(-1):(1);
}
inline double sqr(double arg)
{
return arg * arg;
}
inline double Norm(double arg)
{
return fabs((double)arg);
inline double sqr(double arg) { return arg*arg;}
inline double Norm(double arg) {
return fabs( (double)arg );
}
#if defined(__WIN32__) && !defined(__GNUC__)
inline double hypot(double y, double x)
{
return ::_hypot(y, x);
}
inline double abs(double x)
{
return ::fabs(x);
}
inline double hypot(double y,double x) { return ::_hypot(y,x);}
inline double abs(double x) { return ::fabs(x);}
#endif
// compares whether 2 doubles are equal in an eps-interval.
// Does not check whether a or b represents numbers
// On VC6, if a/b is -INF, it returns false;
inline bool Equal(double a, double b, double eps = epsilon)
inline bool Equal(double a,double b,double eps=epsilon)
{
double tmp = (a - b);
return ((eps > tmp) && (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

284
intern/itasc/kdl/utilities/utility_io.cpp
View File

@@ -4,204 +4,206 @@
/*****************************************************************************
* 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 <ctype.h>
#include <stdlib.h>
#include <ctype.h>
#include <string.h>
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;
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;
ch = is.get();
count++;
_check_istream(is);
if ((prevch == '*') && (ch == '/')) {
break;
}
} while (true);
if (countp != NULL)
*countp = count;
ch = is.get();
return ch;
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();
}
// 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++;
}
else {
ch = (char)is.get();
if (chdescr != Upper(ch)) {
throw Error_BasicIO_Unexpected();
}
}
p++;
}
}
void EatWord(std::istream &is, const char *delim, char *storage, int maxsize)
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();
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);
}
_check_istream(is);
++size;
// ch = is.get();
ch = _EatSpace(is, &count);
}
*p = 0;
is.putback(ch);
*p=0;
is.putback(ch);
}
} // namespace KDL
}

40
intern/itasc/kdl/utilities/utility_io.h
View File

@@ -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,34 +18,38 @@
#ifndef KDL_UTILITY_IO_H_84822
#define KDL_UTILITY_IO_H_84822
// #include <kdl/kdl-config.h>
//#include <kdl/kdl-config.h>
// Standard includes
#include <fstream>
#include <iomanip>
#include <iostream>
#include <iomanip>
#include <fstream>
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...)
@@ -54,16 +58,20 @@ 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

95
intern/itasc/ublas_types.hpp
View File

@@ -9,78 +9,75 @@
#ifndef UBLAS_TYPES_HPP_
#define UBLAS_TYPES_HPP_
#include "kdl/chain.hpp"
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include "kdl/frames.hpp"
#include "kdl/tree.hpp"
#include "kdl/chain.hpp"
#include "kdl/jacobian.hpp"
#include "kdl/jntarray.hpp"
#include "kdl/tree.hpp"
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
namespace iTaSC {
namespace iTaSC{
namespace ublas = boost::numeric::ublas;
using KDL::Chain;
using KDL::Twist;
using KDL::Frame;
using KDL::Inertia;
using KDL::Jacobian;
using KDL::JntArray;
using KDL::Joint;
using KDL::Rotation;
using KDL::Segment;
using KDL::Inertia;
using KDL::SegmentMap;
using KDL::Tree;
using KDL::Twist;
using KDL::JntArray;
using KDL::Jacobian;
using KDL::Segment;
using KDL::Rotation;
using KDL::Vector;
using KDL::Chain;
#define u_scalar double
#define u_vector ublas::vector<u_scalar>
#define u_zero_vector ublas::zero_vector<u_scalar>
#define u_matrix ublas::matrix<u_scalar>
#define u_matrix6 ublas::matrix<u_scalar, 6, 6>
#define u_matrix6 ublas::matrix<u_scalar,6,6>
#define u_identity_matrix ublas::identity_matrix<u_scalar>
#define u_scalar_vector ublas::scalar_vector<u_scalar>
#define u_zero_matrix ublas::zero_matrix<u_scalar>
#define u_vector6 ublas::bounded_vector<u_scalar, 6>
#define u_vector6 ublas::bounded_vector<u_scalar,6>
inline static int changeBase(const u_matrix &J_in, const Frame &T, u_matrix &J_out)
{
inline static int changeBase(const u_matrix& J_in, const Frame& T, u_matrix& J_out) {
if (J_out.size1() != 6 || J_in.size1() != 6 || J_in.size2() != J_out.size2())
return -1;
for (unsigned int j = 0; j < J_in.size2(); ++j) {
ublas::matrix_column<const u_matrix> Jj_in = column(J_in, j);
ublas::matrix_column<u_matrix> Jj_out = column(J_out, j);
Twist arg;
for (unsigned int i = 0; i < 6; ++i)
arg(i) = Jj_in(i);
Twist tmp(T * arg);
for (unsigned int i = 0; i < 6; ++i)
Jj_out(i) = tmp(i);
}
return 0;
if (J_out.size1() != 6 || J_in.size1() != 6 || J_in.size2() != J_out.size2())
return -1;
for (unsigned int j = 0; j < J_in.size2(); ++j) {
ublas::matrix_column<const u_matrix > Jj_in = column(J_in,j);
ublas::matrix_column<u_matrix > Jj_out = column(J_out,j);
Twist arg;
for(unsigned int i=0;i<6;++i)
arg(i)=Jj_in(i);
Twist tmp(T*arg);
for(unsigned int i=0;i<6;++i)
Jj_out(i)=tmp(i);
}
return 0;
}
inline static int changeBase(const ublas::matrix_range<u_matrix> &J_in,
const Frame &T,
ublas::matrix_range<u_matrix> &J_out)
{
inline static int changeBase(const ublas::matrix_range<u_matrix >& J_in, const Frame& T, ublas::matrix_range<u_matrix >& J_out) {
if (J_out.size1() != 6 || J_in.size1() != 6 || J_in.size2() != J_out.size2())
return -1;
for (unsigned int j = 0; j < J_in.size2(); ++j) {
ublas::matrix_column<const ublas::matrix_range<u_matrix>> Jj_in = column(J_in, j);
ublas::matrix_column<ublas::matrix_range<u_matrix>> Jj_out = column(J_out, j);
Twist arg;
for (unsigned int i = 0; i < 6; ++i)
arg(i) = Jj_in(i);
Twist tmp(T * arg);
for (unsigned int i = 0; i < 6; ++i)
Jj_out(i) = tmp(i);
}
return 0;
if (J_out.size1() != 6 || J_in.size1() != 6 || J_in.size2() != J_out.size2())
return -1;
for (unsigned int j = 0; j < J_in.size2(); ++j) {
ublas::matrix_column<const ublas::matrix_range<u_matrix > > Jj_in = column(J_in,j);
ublas::matrix_column<ublas::matrix_range<u_matrix > > Jj_out = column(J_out,j);
Twist arg;
for(unsigned int i=0;i<6;++i)
arg(i)=Jj_in(i);
Twist tmp(T*arg);
for(unsigned int i=0;i<6;++i)
Jj_out(i)=tmp(i);
}
return 0;
}
} // namespace iTaSC
}
#endif /* UBLAS_TYPES_HPP_ */