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];
}
