17 static const unsigned int constraintCacheSize = 5;
19 std::string Armature::m_root =
"root";
56 for (JointConstraintList::iterator it=m_constraints.begin(); it != m_constraints.end(); it++) {
62 m_constraints.clear();
66 segment(_segment), value(), values(), function(_function), y_nr(_y_nr), param(_param), freeParam(_freeParam), substep(_substep)
82 switch (
segment->second.segment.getJoint().getType()) {
131 if (freeParam && param)
143 m_qCCh = m_cache->
addChannel(
this,
"q", m_qKdl.
rows()*
sizeof(
double));
148 m_yCCh = m_cache->
addChannel(
this,
"y", m_nconstraint*constraintCacheSize*
sizeof(
double));
149 m_buf =
new double[m_nconstraint*constraintCacheSize];
158 void Armature::pushQ(
CacheTS timestamp)
168 bool Armature::popQ(
CacheTS timestamp)
173 if (item && m_qCTs != timestamp) {
174 double* q = m_qKdl(0);
175 memcpy(q, item, m_qKdl.
rows()*
sizeof(
double));
180 return (item) ? true :
false;
185 void Armature::pushConstraints(
CacheTS timestamp)
190 double *item = m_buf;
191 for (
unsigned int i=0; i<m_nconstraint; i++) {
192 JointConstraint_struct* pConstraint = m_constraints[i];
193 *item++ = pConstraint->values.feedback;
194 *item++ = pConstraint->values.tolerance;
195 *item++ = pConstraint->value.yd;
196 *item++ = pConstraint->value.yddot;
197 *item++ = pConstraint->values.alpha;
206 bool Armature::popConstraints(
CacheTS timestamp)
210 if (item && m_yCTs != timestamp) {
211 for (
unsigned int i=0; i<m_nconstraint; i++) {
212 JointConstraint_struct* pConstraint = m_constraints[i];
213 if (pConstraint->function != Joint1DOFLimitCallback) {
214 pConstraint->values.feedback = *item++;
215 pConstraint->values.tolerance = *item++;
216 pConstraint->value.yd = *item++;
217 pConstraint->value.yddot = *item++;
218 pConstraint->values.alpha = *item++;
220 item += constraintCacheSize;
225 return (item) ? true :
false;
240 for (
int dof=0; dof<ndof; dof++) {
242 m_joints.push_back(js);
250 SegmentMap::const_iterator sit = m_tree.
getSegment(name);
253 p_joint = &sit->second.segment.getJoint();
254 if (q_size < p_joint->getNDof())
256 p_tip = &sit->second.segment.getFrameToTip();
257 for (
unsigned int dof=0; dof<p_joint->
getNDof(); dof++) {
258 (&q_rest)[dof] = m_joints[sit->second.q_nr+dof].rest;
259 (&q)[dof] = m_qKdl[sit->second.q_nr+dof];
268 double maxJoint = 0.0;
269 for (
unsigned int i=0; i<m_njoint; i++) {
271 double joint =
fabs(m_oldqKdl[i]-m_qKdl[i]);
272 if (maxJoint < joint)
282 double maxDelta = 0.0;
285 for (
unsigned int i = 0; i<m_neffector; i++) {
286 twist =
diff(m_effectors[i].pose, m_effectors[i].oldpose);
288 if (delta > maxDelta)
291 if (delta > maxDelta)
299 SegmentMap::const_iterator segment_it = m_tree.
getSegment(segment_name);
302 if (_freeParam && _param)
306 JointConstraintList::iterator constraint_it;
309 for (iConstraint=0, constraint_it=m_constraints.begin(); constraint_it != m_constraints.end(); constraint_it++, iConstraint++) {
310 pConstraint = *constraint_it;
311 if (pConstraint->
segment == segment_it) {
317 pConstraint->
param = _param;
319 pConstraint->
substep = _substep;
324 if (_freeParam && _param)
330 m_constraints.push_back(pConstraint);
331 m_noutput += pConstraint->
v_nr;
332 return m_nconstraint++;
337 SegmentMap::const_iterator segment_it = m_tree.
getSegment(segment_name);
340 const Joint& joint = segment_it->second.segment.getJoint();
345 if ((joint.
getNDof() == 1 && dof > 0) || (joint.
getNDof() == 2 && dof > 1))
347 Joint_struct& p_joint = m_joints[segment_it->second.q_nr+dof];
357 if (segments.find(name) == segments.end())
360 EffectorList::const_iterator it;
362 for (it=m_effectors.begin(), ee=0; it!=m_effectors.end(); it++, ee++) {
363 if (it->name == name)
369 m_effectors.push_back(effector);
370 return m_neffector++;
375 unsigned int i, j,
c;
381 for (i=
c=0; i<m_nconstraint; i++) {
383 for (j=0; j<pConstraint->
v_nr; j++,
c++) {
392 m_oldqKdl.
resize(m_njoint);
393 m_newqKdl.
resize(m_njoint);
394 m_qdotKdl.
resize(m_njoint);
395 for (i=0; i<m_njoint; i++) {
396 m_newqKdl[i] = m_oldqKdl[i] = m_qKdl[i] = m_joints[i].rest;
402 for (i=0; i<m_neffector; i++) {
404 KDL::SegmentMap::value_type
const *sit = m_tree.
getSegmentPtr(m_effectors[i].name);
405 while (sit->first !=
"root") {
406 Frame tip = sit->second.segment.pose(m_qKdl(sit->second.q_nr));
408 sit = sit->second.parent;
451 double* qdot=m_qdotKdl(0);
453 double* newq=m_newqKdl(0);
454 double norm, qx, qz, CX, CZ,
sx, sz;
458 for (q_nr=0; q_nr<
m_nq; ++q_nr)
461 for (q_nr=0; q_nr<
m_nq; ) {
464 switch (joint->
type) {
469 if (joint[0].useLimit) {
470 if (joint[1].useLimit) {
476 if (qx > 0.0 && qz > 0.0) {
479 }
else if (qx <= 0.0 && qz > 0.0) {
484 }
else if (qx <= 0.0 && qz <= 0.0) {
514 newq[1] = qz*
norm*CZ*sz;
521 if (qx > joint[0].
max) {
522 newq[0] = joint[0].
max;
524 }
else if (qx < joint[0].
min) {
525 newq[0] = joint[0].
min;
529 }
else if (joint[1].useLimit) {
532 if (qz > joint[1].
max) {
533 newq[1] = joint[1].
max;
535 }
else if (qz < joint[1].
min) {
536 newq[1] = joint[1].
min;
540 if (joint[0].locked) {
553 callback.lockJoint(q_nr, 2, deltaq);
569 for (
unsigned int i=0; i<joint->
ndof; i++) {
571 if (joint[i].useLimit) {
572 if (newq[i] > joint[i].
max) {
573 newq[i] = joint[i].
max;
575 }
else if (newq[i] < joint[i].
min) {
576 newq[i] = joint[i].
min;
581 if (joint[0].locked) {
585 for (
unsigned int i=0; i<joint->
ndof; i++) {
586 qdot[i] = newq[i] - q[i];
587 norm += qdot[i]*qdot[i];
594 for (
unsigned int i=0; i<joint->
ndof; i++) {
611 for ( ; q_nr<
m_nq; ) {
618 return (unlocked) ? locked :
false;
628 memcpy(m_qKdl(0), m_newqKdl(0),
sizeof(
double)*m_qKdl.
rows());
638 for (
unsigned int ee=0; ee<
m_nee; ee++) {
639 m_fksolver->
JntToCart(m_qKdl,m_effectors[ee].pose,m_effectors[ee].name,m_root);
640 m_jacsolver->
JntToJac(m_qKdl,*m_jac,m_effectors[ee].name);
645 for(
unsigned int i=0;i<6;i++) {
646 for(
unsigned int j=0;j<
m_nq;j++)
647 Jq(i,j)=(*m_jac)(i,j);
665 return (m_fksolver->
JntToCart(m_qKdl,
result,segment_name,base_name) < 0) ? false :
true;
681 memcpy(m_oldqKdl(0), m_qKdl(0),
sizeof(
double)*m_qKdl.
rows());
682 for (
unsigned int i=0; i<m_neffector; i++) {
683 m_effectors[i].oldpose = m_effectors[i].pose;
688 for (JointList::iterator
jit=m_joints.begin();
jit!=m_joints.end(); ++
jit) {
689 (*jit).locked =
false;
692 JointConstraintList::iterator it;
693 unsigned int iConstraint;
696 for (iConstraint=0, it=m_constraints.begin(); it!=m_constraints.end(); it++, iConstraint++) {
699 for (i=0, nr = pConstraint->
segment->second.q_nr; i<pConstraint->v_nr; i++, nr++) {
700 *(
double *)&pConstraint->
value[i].
y = m_qKdl[nr];
701 *(
double *)&pConstraint->
value[i].
ydot = m_qdotKdl[nr];
707 for (i=0, nr=pConstraint->
y_nr; i<pConstraint->v_nr; i++, nr++) {
716 unsigned int lastid, i;
719 lastid = m_nconstraint;
720 }
else if (constraintId < m_nconstraint) {
721 lastid = constraintId+1;
725 for ( ; constraintId<lastid; ++constraintId) {
728 for (i=0; i<pConstraint->
v_nr; i++) {
744 for (i=0; i<pConstraint->
v_nr; i++) {
745 if (valueId == pConstraint->
value[i].
id) {
748 pConstraint->
value[i].
yd = value;
769 for (i=0; i<pConstraint->
v_nr; i++)
void BLI_kdtree_nd_() free(KDTree *tree)
#define CONSTRAINT_ID_ALL
SIMD_FORCE_INLINE btScalar norm() const
Return the norm (length) of the vector.
represents a frame transformation in 3D space (rotation + translation)
Vector p
origine of the Frame
unsigned int rows() const
void resize(unsigned int newSize)
This class encapsulates a simple joint, that is with one parameterized degree of freedom and with sca...
unsigned int getNDof() const
const JointType & getType() const
represents rotations in 3 dimensional space.
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and an inertia) with...
virtual int JntToCart(const JntArray &q_in, Frame &p_out, const std::string &segmentName, const std::string &baseName)
int JntToJac(const JntArray &q_in, Jacobian &jac, const std::string &segmentname)
const SegmentMap & getSegments() const
SegmentMap::const_iterator getSegment(const std::string &segment_name) const
SegmentMap::value_type const * getSegmentPtr(const std::string &segment_name) const
bool addSegment(const Segment &segment, const std::string &segment_name, const std::string &hook_name)
represents both translational and rotational velocities.
Vector rot
The rotational velocity of that point.
Vector vel
The velocity of that point.
A concrete implementation of a 3 dimensional vector class.
virtual void initCache(Cache *_cache)
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())
int addLimitConstraint(const std::string &segment_name, unsigned int dof, double _min, double _max)
virtual void updateControlOutput(const Timestamp ×tamp)
virtual void pushCache(const Timestamp ×tamp)
virtual bool updateJoint(const Timestamp ×tamp, JointLockCallback &callback)
virtual void updateKinematics(const Timestamp ×tamp)
bool getRelativeFrame(Frame &result, const std::string &segment_name, const std::string &base_name=m_root)
virtual const Frame & getPose(const unsigned int end_effector)
int addConstraint(const std::string &segment_name, ConstraintCallback _function, void *_param=NULL, bool _freeParam=false, bool _substep=false)
virtual int addEndEffector(const std::string &name)
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)
virtual void updateJacobian()
virtual bool setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, double value, double timestep=0.0)
double getMaxEndEffectorChange()
virtual bool setJointArray(const KDL::JntArray &joints)
double getMaxJointChange()
virtual const KDL::JntArray & getJointArray()
int addChannel(const void *device, const char *name, unsigned int maxItemSize)
double * addCacheVectorIfDifferent(const void *device, int channel, CacheTS timestamp, double *data, unsigned int length, double threshold)
const void * getPreviousCacheItem(const void *device, int channel, CacheTS *timestamp)
std::vector< e_matrix > m_JqArray
virtual void initialize(unsigned int _nq, unsigned int _nc, unsigned int _nee)
DEGForeachIDComponentCallback callback
IMETHOD Rotation Rot(const Vector &axis_a_b)
ccl_gpu_kernel_postfix ccl_global float int sx
ccl_device_inline float2 fabs(const float2 &a)
Segment< FEdge *, Vec3r > segment
IMETHOD Vector diff(const Vector &a, const Vector &b, double dt=1)
double epsilon2
power or 2 of epsilon
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
std::map< std::string, TreeElement, std::less< std::string >, Eigen::aligned_allocator< std::pair< const std::string, TreeElement > > > SegmentMap
void changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest)
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
INLINE Rall1d< T, V, S > sqr(const Rall1d< T, V, S > &arg)
T length(const vec_base< T, Size > &a)
bool(* ConstraintCallback)(const Timestamp ×tamp, struct ConstraintValues *const _values, unsigned int _nvalues, void *_param)
JointConstraint_struct(SegmentMap::const_iterator _segment, unsigned int _y_nr, ConstraintCallback _function, void *_param, bool _freeParam, bool _substep)
ConstraintSingleValue value[3]
SegmentMap::const_iterator segment
ConstraintCallback function
ConstraintValues values[3]
~JointConstraint_struct()
KDL::Joint::JointType type
struct ConstraintSingleValue * values