void SelfCollisionConstraint::Eval(const Vector& x, Vector& v) { int n=NumDimensions(); Assert(v.n == n); for(int i=0;i<n;i++) { v(i) = Eval_i(x,i); } }
CSpaceOMPLStateSpace::CSpaceOMPLStateSpace(CSpace* _space,CSpaceOMPLSpaceInformation* _si) :ob::RealVectorStateSpace(NumDimensions(_space)),space(_space),si(_si) { PropertyMap props; space->Properties(props); vector<double> minimum,maximum; if(props.getArray("minimum",minimum) && props.getArray("maximum",maximum)) { } else { minimum.resize(0); maximum.resize(0); } }
void Element::SetType(ElType el_type) { m_ElType = el_type; m_NumElements = 1; for (uint i=0; i<NumDimensions(); i++) m_NumElements *= Dimension(i); switch (m_ElType) { case Char: case Uchar: m_Align = 1; m_Size = m_Align * m_NumElements; break; case Short: case Ushort: m_Align = 2; m_Size = m_Align * m_NumElements; break; case Int: case Uint: case Float: m_Align = 4; m_Size = m_Align * m_NumElements; break; case Double: m_Align = 8; m_Size = m_Align * m_NumElements; break; case Struct: if (m_Struct) { m_Align = m_Struct->Align(); m_Size = m_Struct->Size() * m_NumElements; } break; default: m_Align = 0; m_Size = 0; break; } }
void SelfCollisionConstraint::Jacobian(const Vector& x,Matrix& J) { int n=NumDimensions(); Assert(J.hasDims(n,robot.links.size())); Vector3 cpa,cpb,dir; //Vector3 cpa_world,cpb_world; Vector3 dv; Real dirNorm=Zero; J.setZero(); for(int i=0;i<n;i++) { int a=collisionPairs[i].first,b=collisionPairs[i].second; if(query[i].ClosestPoints(cpa,cpb,dir)) { dirNorm = dir.norm(); int lca = robot.LCA(a,b); //jacobian Ji is (Jpa*dir-Jpb*dir)/|dir| for(int j=a;j!=lca;j=robot.parents[j]) { robot.GetPositionJacobian(cpa,a,j,dv); if(dirNorm < Epsilon) J(i,j) = dot(dv,dv); else J(i,j) = dot(dir,dv) / dirNorm; } for(int j=b;j!=lca;j=robot.parents[j]) { robot.GetPositionJacobian(cpb,b,j,dv); if(dirNorm < Epsilon) J(i,j) -= dot(dv,dv); else J(i,j) -= dot(dir,dv) / dirNorm; } } } }
unsigned int CSpaceOMPLStateSpace::getDimension (void) const { return NumDimensions(space); }
uint Element::Dimension(uint dim_num) const { assert(dim_num < NumDimensions()); return m_Dimension[dim_num]; }