Exemple #1
0
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);
  }
Exemple #5
0
/////////////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;
}
Exemple #6
0
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);
  }
}
Exemple #7
0
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;
 }