const ValVec<htmRange> &
htmInterface::doHull() {
if(polyCorners_.length() < 3)
throw SpatialInterfaceError("htmInterface:convexHull: empty hull: points on one line");
SpatialVector v;
SpatialConvex x;
SpatialDomain d;
// The constraint we have for each side is a 0-constraint (great circle)
// passing through the 2 corners. Since we are in counterclockwise order,
// the vector product of the two successive corners just gives the correct
// constraint.
size_t i, len = polyCorners_.length();
for(i = 0; i < len; i++) {
v = polyCorners_[i].c_ ^ polyCorners_[ i == len-1 ? 0 : i + 1].c_;
#ifdef DIAG
cerr << v << " " << i << "," << i+1 << "\n";
#endif
v.normalize();
SpatialConstraint c(v,0);
x.add(c);
}
d.add(x);
d.intersect(index_, idList_);
range_.cut(range_.length());
makeRange();
return range_;
}
const ValueVector &
htmInterface::doHull() {
if(polyCorners_.size() < 3)
throw SpatialInterfaceError("htmInterface:convexHull: empty hull: points on one line");
SpatialVector v;
RangeConvex cvx;
SpatialDomain dom;
// The constraint we have for each side is a 0-constraint (great circle)
// passing through the 2 corners. Since we are in counterclockwise order,
// the vector product of the two successive corners just gives the correct
// constraint.
size_t i, len = polyCorners_.size();
for(i = 0; i < len; i++) {
v = polyCorners_[i].c_ ^ polyCorners_[ i == len-1 ? 0 : i + 1].c_;
#ifdef DIAG
cerr << "doHull:: " << v << " " << i << "," << i+1 << endl;
#endif
v.normalize();
SpatialConstraint c(v,0);
cvx.add(c); // [ed:RangeConvex::add]
}
dom.add(cvx);
//%%%%%%%%%%%%%%%%%%%%%%%%%%%
// dom.convexes_[0].boundingCircle_.write(cout);
// dom.write(cout);
//%%%%%%%%%%%%%%%%%%%%%%%%%%%
return domain(dom);
}
WallTransferEventProcessor::WallTransferEventProcessor(IEventProvider* eventProvider,
const Model::SystemConfig& config) : BaseEventProcessor(eventProvider)
{
this->config = &config;
SpatialVector minVertexCoordinates;
minVertexCoordinates.assign(0);
box.Initialize(minVertexCoordinates, config.packingSize);
}
SpatialVector<T> CrossProduct(const SpatialVector<T> rhVector)
{
T x = Y() * rhVector.Z() - Z() * rhVector.Y();
T y = Z() * rhVector.X() - X() * rhVector.Z();
T z = X() * rhVector.Y() - Y() * rhVector.X();
return SpatialVector<T> (x,y,z);
}
/////////////TESTBOUNDINGCIRCLE///////////////////////////
// testBoundingCircle: test for boundingCircles intersecting with constraint
//
bool
RangeConvex::testBoundingCircle(const SpatialVector & v0,
const SpatialVector & v1,
const SpatialVector & v2) {
// Set the correct direction: The normal vector to the triangle plane
SpatialVector c = ( v1 - v0 ) ^ ( v2 - v1 );
c.normalize();
// Set the correct opening angle: Since the plane cutting out the triangle
// also correctly cuts out the bounding cap of the triangle on the sphere,
// we can take any corner to calculate the opening angle
float64 d = acos (c * v0);
// for zero convexes, we have calculated a bounding circle for the convex.
// only test with this single bounding circle.
if(sign_ == zERO) {
float64 tst;
if ( ( (tst = c * boundingCircle_.a_) < -1.0L + gEpsilon ? gPi :
acos(tst) ) >
( d + boundingCircle_.s_) ) return false;
return true;
}
// for all other convexes, test every constraint. If the bounding
// circle lies completely outside of one of the constraints, reject.
// else, accept.
size_t i;
for(i = 0; i < constraints_.size(); i++) {
if ( ( (c * constraints_[i].a_) < -1.0L + gEpsilon ? gPi :
acos(c * constraints_[i].a_) ) >
( d + constraints_[i].s_) ) return false;
}
return true;
}
void NewtonEulerSolver::CalcAccel(const Vector& t,Vector& ddq)
{
ddq.resize(robot.links.size());
inertiaMatrices.resize(robot.links.size());
biasingForces.resize(robot.links.size());
CalcVelocities();
//initialize the matrices
Matrix3 Iworld;
//velocity dependent accelerations (centrifugal and coriolis terms)
//calculated at center of mass!
vector<SpatialVector> velDepAccels(robot.links.size());
for(size_t n=0;n<robot.links.size();n++) {
//cout<<"Velocity n: "<<velocities[n].v<<", "<<velocities[n].w<<endl;
//cout<<" Vel at com: "<<velocities[n].v + cross(velocities[n].w,robot.links[n].T_World.R*robot.links[n].com)<<endl;
robot.links[n].GetWorldInertia(Iworld);
inertiaMatrices[n].setMassMatrix(robot.links[n].mass,Iworld);
Vector3 gyroscopicForce = cross(velocities[n].w,Iworld*velocities[n].w);
int p = robot.parents[n];
if(p < 0) {
const Vector3 &w2=velocities[n].w;
Vector3 com_n_local = robot.links[n].T_World.R*robot.links[n].com;
Vector3 v = cross(w2,cross(w2,com_n_local));
velDepAccels[n].set(v,Vector3(Zero));
}
else {
//calculate these quantities about the center of mass of each link
Vector3 com_n_local = robot.links[n].T_World.R*robot.links[n].com;
Vector3 com_p_local = robot.links[p].T_World.R*robot.links[p].com;
Vector3 v1=velocities[p].v+cross(velocities[p].w,com_p_local);
Vector3 v2=velocities[n].v+cross(velocities[n].w,com_n_local);
const Vector3 &w1=velocities[p].w, &w2=velocities[n].w;
Vector3 cdiff = robot.links[n].T_World*robot.links[n].com-robot.links[p].T_World*robot.links[p].com;
Vector3 v = cross(w1,v2-v1);
if(robot.links[n].type == RobotLink3D::Prismatic) {
v += robot.dq(n)*cross(w1,robot.links[p].T_World.R*robot.links[n].w);
}
else {
v += cross(w2,cross(w2-w1,com_n_local));
}
Vector3 w = cross(w1,w2);
velDepAccels[n].set(v,w);
}
biasingForces[n].set(-externalWrenches[n].f,gyroscopicForce-externalWrenches[n].m);
//cout<<"Velocity dependent accel: "<<velDepAccels[n]<<endl;
}
//go backward down the list
SpatialMatrix cToP,pToC,IaaI,temp,temp2;
SpatialVector A,Ia,bf_Ivda,vtemp;
for(int n=(int)robot.links.size()-1;n>=0;--n) {
if(children[n].empty()) continue;
Vector3 com_n = robot.links[n].T_World*robot.links[n].com;
for(size_t i=0;i<children[n].size();i++) {
int c = children[n][i];
//compute the transformations
Vector3 com_c_local =robot.links[c].T_World.R*robot.links[c].com;
Vector3 com_c = com_c_local + robot.links[c].T_World.t;
cToP.setForceShift(com_c,com_n);
pToC.setVelocityShift(com_n,com_c);
//compute quantities with A
Vector3 axis_w=robot.links[c].T_World.R*robot.links[c].w;
if(robot.links[c].type == RobotLink3D::Revolute)
A.set(cross(axis_w,com_c_local),axis_w);
else
A.set(axis_w,Vector3(Zero));
inertiaMatrices[c].mul(A,Ia);
Real aIa = Ia.dot(A);
Assert(aIa > Zero);
//IaaI.setOuterProduct(Ia);
for(int i=0;i<6;i++)
for(int j=0;j<6;j++)
IaaI(i,j) = Ia(i)*Ia(j);
//add this child's contribution to the revised inertia matrix
//I[n] += cToP*(I-Ia*Iat/aIa)*pToC
temp = inertiaMatrices[c];
temp.madd(IaaI,-1.0/aIa);
temp2.mul(cToP,temp);
temp.mul(temp2,pToC);
inertiaMatrices[n] += temp;
//add this child's contribution to the revised biasing force
//bf[n] += cToP*(bf+I*vda + I*A*(t[c]-At*(bf+I*vda))/aIa)
bf_Ivda = biasingForces[c];
inertiaMatrices[c].madd(velDepAccels[c],bf_Ivda);
Real At_bf_Ivda = A.dot(bf_Ivda);
vtemp.mul(Ia,(t[c]-At_bf_Ivda)/aIa);
vtemp += bf_Ivda;
cToP.madd(vtemp,biasingForces[n]);
}
//cout<<"Revised inertia matrix "<<n<<": "<<endl<<MatrixPrinter(inertiaMatrices[n])<<endl;
//cout<<"Revised biasing force "<<n<<": "<<endl<<VectorPrinter(biasingForces[n])<<endl;
//getchar();
}
//go down the heirarchy, computing accelerations along the way
//joint force is X[cm->origin]*(I*a+bf)
//NOTE: acceleration variables are in com frame, transform them later
SpatialVector nAccel,pAccel;
for(size_t n=0;n<robot.links.size();n++) {
Vector3 com_n_local =robot.links[n].T_World.R*robot.links[n].com;
//compute quantities with A
Vector3 axis_w=robot.links[n].T_World.R*robot.links[n].w;
if(robot.links[n].type == RobotLink3D::Revolute)
A.set(cross(axis_w,com_n_local),axis_w);
else
A.set(axis_w,Vector3(Zero));
inertiaMatrices[n].mul(A,Ia);
Real aIa = Ia.dot(A);
Assert(aIa > Zero);
int p=robot.parents[n];
if(p>=0) {
vtemp.set(accelerations[p].v,accelerations[p].w);
Vector3 com_n = robot.links[n].T_World*robot.links[n].com;
Vector3 com_p = robot.links[p].T_World*robot.links[p].com;
//which way, + or -?
pToC.setVelocityShift(com_p,com_n);
pToC.mul(vtemp,pAccel);
}
else
pAccel.setZero();
//ddq = (t-A^t*I*X[p->n]*a[p] - A^t(bf+I*vda))/(A^tIA)
bf_Ivda = biasingForces[n];
inertiaMatrices[n].madd(velDepAccels[n],bf_Ivda);
ddq(n) = t(n)-Ia.dot(pAccel);
ddq(n) -= A.dot(bf_Ivda);
ddq(n) /= aIa;
//a = X[p->n]*a[p] + vda + q''*A
vtemp = velDepAccels[n];
vtemp.madd(A,ddq(n));
vtemp += pAccel;
//vtemp is now the acceleration about the center of mass
vtemp.get(accelerations[n].v,accelerations[n].w);
//cout<<"CM acceleration: "<<vn.v<<", "<<vn.w<<endl;
}
for(size_t n=0;n<robot.links.size();n++) {
Vector3 com_n_local =robot.links[n].T_World.R*robot.links[n].com;
//transform to origin
RigidBodyVelocity vn=accelerations[n];
accelerations[n].setShifted(vn,-com_n_local);
SpatialVector jointForce = biasingForces[n];
inertiaMatrices[n].madd(vtemp,jointForce);
//now jointForce is in the com frame
Wrench fn;
jointForce.get(fn.f,fn.m);
//transform to joint frame
jointWrenches[n].setShifted(fn,-com_n_local);
}
}
void
RangeConvex::simplify0() {
size_t i,j,k;
SpatialVector vi1, vi2;
typedef std::vector<size_t> ValueVectorSzt;
ValueVectorSzt cornerConstr1, cornerConstr2, removeConstr;
ValueVectorSpvec corner;
if (constraints_.size() == 1) { // for one constraint, it is itself the BC
boundingCircle_ = constraints_[0];
return;
// For 2 constraints, take the bounding circle a 0-constraint...
// this is by no means optimal, but the code is optimized for at least
// 3 zERO constraints... so this is acceptable.
} else if(constraints_.size() == 2) {
// test for constraints being identical - rule 1 out
if(constraints_[0].a_ == constraints_[1].a_){
constraints_.erase(constraints_.end()-1);
boundingCircle_ = constraints_[0];
return;
}
// test for constraints being two disjoint half spheres - empty convex!
if(constraints_[0].a_ == (-1.0)*constraints_[1].a_){
constraints_.clear();
return;
}
boundingCircle_ = SpatialConstraint(constraints_[0].v() +
constraints_[1].v(),0);
return;
}
// Go over all pairs of constraints
for(i = 0; i < constraints_.size() - 1; i++) {
bool ruledout = true;
for(j = i+1; j < constraints_.size(); j ++) {
// test for constraints being identical - rule i out
if(constraints_[i].a_ == constraints_[j].a_) break;
// test for constraints being two disjoint half spheres - empty convex!
if(constraints_[i].a_ == (-1.0)*constraints_[j].a_){
constraints_.clear();
return;
}
// vi1 and vi2 are their intersection points
vi1 = constraints_[i].a_ ^ constraints_[j].a_ ;
vi1.normalize();
vi2 = (-1.0) * vi1;
bool vi1ok = true, vi2ok = true;
// now test whether vi1 or vi2 or both are inside every other constraint.
// if yes, store them in the corner array.
for(k = 0; k < constraints_.size(); k++) {
if(k == i || k == j) continue;
if(vi1ok && vi1 * constraints_[k].a_ <= 0.0) vi1ok = false;
if(vi2ok && vi2 * constraints_[k].a_ <= 0.0) vi2ok = false;
if(!vi1ok && !vi2ok) break;
}
if(vi1ok) {
corner.push_back(vi1);
cornerConstr1.push_back(i);
cornerConstr2.push_back(j);
ruledout = false;
}
if(vi2ok) {
corner.push_back(vi2);
cornerConstr1.push_back(i);
cornerConstr2.push_back(j);
ruledout = false;
}
}
// is this constraint ruled out? i.e. none of its intersections
// with other constraints are corners... remove it from constraints_ list.
if(ruledout) removeConstr.push_back(i);
}
// Now set the corners into their correct order, which is an
// anti-clockwise walk around the polygon.
//
// start at any corner. so take the first.
corners_.clear();
corners_.push_back(corner[0]);
// The trick is now to start off into the correct direction.
// this corner has two edges it can walk. we have to take the
// one where the convex lies on its left side.
i = cornerConstr1[0]; // the i'th constraint and j'th constraint
j = cornerConstr2[0]; // intersect at 0'th corner
size_t c1,c2,k1,k2;
// Now find the other corner where the i'th and j'th constraints intersect.
// Store the corner in vi1 and vi2, and the other constraint indices
// in c1,c2.
for( k = 1; k < cornerConstr1.size(); k ++) {
if(cornerConstr1[k] == i) {
vi1 = corner[k];
c1 = cornerConstr2[k];
k1 = k;
}
if(cornerConstr2[k] == i) {
vi1 = corner[k];
c1 = cornerConstr1[k];
k1 = k;
}
if(cornerConstr1[k] == j) {
vi2 = corner[k];
c2 = cornerConstr2[k];
k2 = k;
}
if(cornerConstr2[k] == j) {
vi2 = corner[k];
c2 = cornerConstr1[k];
k2 = k;
}
}
// Now test i'th constraint-edge ( corner 0 and corner k ) whether
// it is on the correct side (left)
//
// ( (corner(k) - corner(0)) x constraint(i) ) * corner(0)
//
// is >0 if yes, <0 if no...
//
size_t c,currentCorner;
if( ((vi1 - corner[0]) ^ constraints_[i].a_) * corner[0] > 0 ) {
corners_.push_back(vi1);
c = c1;
currentCorner = k1;
} else {
corners_.push_back(vi2);
c = c2;
currentCorner = k2;
}
// now append the corners that match the index c until we got corner 0 again
// currentCorner holds the current corners index
// c holds the index of the constraint that has just been intersected with
// So:
// x We are on a constraint now (i or j from before), the second corner
// is the one intersecting with constraint c.
// x Find other corner for constraint c.
// x Save that corner, and set c to the constraint that intersects with c
// at that corner. Set currentcorner to that corners index.
// x Loop until 0th corner reached.
while( currentCorner ) {
for (k = 0; k < cornerConstr1.size(); k++) {
if(k == currentCorner)continue;
if(cornerConstr1[k] == c) {
if( (currentCorner = k) == 0) break;
corners_.push_back(corner[k]);
c = cornerConstr2[k];
break;
}
if(cornerConstr2[k] == c) {
if( (currentCorner = k) == 0) break;
corners_.push_back(corner[k]);
c = cornerConstr1[k];
break;
}
}
}
// Remove all redundant constraints
for ( i = 0; i < removeConstr.size(); i++)
constraints_.erase(constraints_.end()-removeConstr[i]-1);
// Now calculate the bounding circle for the convex.
// We take it as the bounding circle of the triangle with
// the widest opening angle. All triangles made out of 3 corners
// are considered.
boundingCircle_.d_ = 1.0;
if (constraints_.size() >=3 ) {
for(i = 0; i < corners_.size(); i++)
for(j = i+1; j < corners_.size(); j++)
for(k = j+1; k < corners_.size(); k++) {
SpatialVector v = ( corners_[j] - corners_[i] ) ^
( corners_[k] - corners_[j] );
v.normalize();
// Set the correct opening angle: Since the plane cutting
// out the triangle also correctly cuts out the bounding cap
// of the triangle on the sphere, we can take any corner to
// calculate the opening angle
float64 d = v * corners_[i];
if(boundingCircle_.d_ > d) boundingCircle_ = SpatialConstraint(v,d);
}
}
#ifdef DIAGNOSE
for(i = 0; i < corners_.size(); i++) {
cout << corners_[i].ra() << "," << corners_[i].dec() << ":" << corners_[i] << endl;
}
#endif
}
SpatialVector<T> operator-(const SpatialVector<T>& arg2) const
{
return SpatialVector<T>(X() - arg2.X(), Y() - arg2.Y(), Z() - arg2.Z());
}
SpatialVector<T> operator+(const SpatialVector<T>& arg2)
{
return SpatialVector<T>(X() + arg2.X(), Y() + arg2.Y(), Z() + arg2.Z());
}
T InnerProduct(const SpatialVector<T> rhVector)
{
T retVal = boost::initialized_value;
retVal = X()*rhVector.X() + Y()*rhVector.Y() + Z()*rhVector.Z();
return retVal;
}