void IKGoal::RemoveRotationAxis(const Vector3& p,const Vector3& axis) { switch(rotConstraint) { case 0: //done break; case 1: //single-axis rotation FatalError("TODO - set dual-axis rotation"); break; case 2: //dual-axis rotation //if one of the axes are parallel to the normal, all is well FatalError("TODO - dual-axis rotation isn't implemented"); break; case 3: //fixed rotation //allow it to rotate about the cp normal { Vector3 locAxis; Vector3 locPos; RigidTransform T; GetFixedGoalTransform(T); T.R.mulTranspose(axis,locAxis); T.mulInverse(p,locPos); SetAxisRotation(locAxis,axis); localPosition = locPos; endPosition = p; } break; } }
void IKSolver::getResidual(std::vector<double>& out) { int size = 0; for(size_t i=0;i<objectives.size();i++) { int m=IKGoal::NumDims(objectives[i].goal.posConstraint); int n=IKGoal::NumDims(objectives[i].goal.rotConstraint); size += m + n; } out.resize(size); size = 0; for(size_t i=0;i<objectives.size();i++) { const IKGoal& goal = objectives[i].goal; int m=IKGoal::NumDims(goal.posConstraint); int n=IKGoal::NumDims(goal.rotConstraint); Real poserr[3],orierr[3]; if(goal.destLink < 0) goal.GetError(robot.robot->links[goal.link].T_World,poserr,orierr); else { RigidTransform Trel; Trel.mulInverseB(robot.robot->links[goal.link].T_World,robot.robot->links[goal.destLink].T_World); goal.GetError(Trel,poserr,orierr); } for(int k=0;k<m;k++,size++) out[size] = poserr[k]; for(int k=0;k<n;k++,size++) out[size] = orierr[k]; } }
void NearbyPoints(const CollisionPointCloud& pc,const GeometricPrimitive3D& g,Real tol,std::vector<int>& pointIds,size_t maxContacts) { Box3D bb; GetBB(pc,bb); //quick reject test if(g.Distance(bb) > tol) return; GeometricPrimitive3D glocal = g; RigidTransform Tinv; Tinv.setInverse(pc.currentTransform); glocal.Transform(Tinv); AABB3D gbb = glocal.GetAABB(); gbb.setIntersection(pc.bblocal); //octree overlap method vector<Vector3> points; vector<int> ids; pc.octree->BoxQuery(gbb.bmin-Vector3(tol),gbb.bmax+Vector3(tol),points,ids); for(size_t i=0;i<points.size();i++) if(glocal.Distance(points[i]) <= tol) { pointIds.push_back(ids[i]); if(pointIds.size()>=maxContacts) return; } return; /* //grid enumeration method GridSubdivision::Index imin,imax; pc.grid.PointToIndex(Vector(3,gbb.bmin),imin); pc.grid.PointToIndex(Vector(3,gbb.bmax),imax); int numCells = (imax[0]-imin[0]+1)*(imax[1]-imin[1]+1)*(imax[2]-imin[2]+1); if(numCells > (int)pc.points.size()) { printf("Testing all points\n"); //test all points, linearly for(size_t i=0;i<pc.points.size();i++) if(glocal.Distance(pc.points[i]) <= tol) { pointIds.push_back(int(i)); if(pointIds.size()>=maxContacts) return; } } else { printf("Testing points in BoxQuery\n"); gNearbyTestThreshold = tol; gNearbyTestResults.resize(0); gNearbyTestObject = &glocal; gNearbyTestBranch = maxContacts; pc.grid.BoxQuery(Vector(3,gbb.bmin),Vector(3,gbb.bmax),nearbyTest); pointIds.resize(gNearbyTestResults.size()); for(size_t i=0;i<points.size();i++) pointIds[i] = gNearbyTestResults[i] - &pc.points[0]; } */ }
/// \brief transform the center of mass and inertia matrix by trans void OrXmlMass::ChangeCoordinateSystem(RigidTransform& trans) { // Vector oldcom = t.trans; // TransformMatrix trot = matrixFromQuat(trans.rot); // t = trot.rotate(t.rotate(trot.inverse())); // rotate inertia // t.trans = trans*oldcom; if(!com || !inertia){ cout<<"No com or inertia specified!"<<flush; getchar(); } Vector3 oldcom(*com); RigidTransform trot, inverseT, massT, tmpT, finalmassT; trot.R.set(trans.R); trot.t.setZero(); massT.R.set(*inertia); massT.t.set(*com); trot.getInverse(inverseT); tmpT.mul(massT, inverseT); finalmassT.mul(trot, tmpT); trans.mul(oldcom, finalmassT.t); com->set(finalmassT.t); inertia->set(finalmassT.R); }
void WorkspaceBound::SetTransformed(const WorkspaceBound& w, RigidTransform& T) { T.mulPoint(w.center,center); outerRadius = w.outerRadius; innerRadius = w.innerRadius; maxAngle = w.maxAngle; }
void WorkspaceBound::SetTransformed(const WorkspaceBound& w, RigidTransform& T) { balls.resize(w.balls.size()); for(size_t i=0;i<w.balls.size();i++) { T.mulPoint(w.balls[i].center,balls[i].center); balls[i].outerRadius = w.balls[i].outerRadius; balls[i].innerRadius = w.balls[i].innerRadius; } maxAngle = w.maxAngle; }
bool WithinDistance(const CollisionPointCloud& pc,const GeometricPrimitive3D& g,Real tol) { Box3D bb; GetBB(pc,bb); //quick reject test if(g.Distance(bb) > tol) return false; GeometricPrimitive3D glocal = g; RigidTransform Tinv; Tinv.setInverse(pc.currentTransform); glocal.Transform(Tinv); AABB3D gbb = glocal.GetAABB(); gbb.setIntersection(pc.bblocal); //octree overlap method vector<Vector3> points; vector<int> ids; pc.octree->BoxQuery(gbb.bmin-Vector3(tol),gbb.bmax+Vector3(tol),points,ids); for(size_t i=0;i<points.size();i++) if(glocal.Distance(points[i]) <= tol) return true; return false; /* //grid enumeration method GridSubdivision::Index imin,imax; pc.grid.PointToIndex(Vector(3,gbb.bmin),imin); pc.grid.PointToIndex(Vector(3,gbb.bmax),imax); int numCells = (imax[0]-imin[0]+1)*(imax[1]-imin[1]+1)*(imax[2]-imin[2]+1); if(numCells > (int)pc.points.size()) { //test all points, linearly for(size_t i=0;i<pc.points.size();i++) if(glocal.Distance(pc.points[i]) <= tol) return true; return false; } else { gWithinDistanceTestThreshold = tol; gWithinDistanceTestObject = &glocal; bool collisionFree = pc.grid.IndexQuery(imin,imax,withinDistanceTest); return !collisionFree; } */ }
Real Distance(const CollisionPointCloud& pc,const GeometricPrimitive3D& g) { GeometricPrimitive3D glocal = g; RigidTransform Tinv; Tinv.setInverse(pc.currentTransform); glocal.Transform(Tinv); /* AABB3D gbb = glocal.GetAABB(); gDistanceTestValue = Inf; gDistanceTestObject = &glocal; pc.grid.BoxQuery(Vector(3,gbb.bmin),Vector(3,gbb.bmax),distanceTest); return gDistanceTestValue; */ //test all points, linearly Real dmax = Inf; for(size_t i=0;i<pc.points.size();i++) dmax = Min(dmax,glocal.Distance(pc.points[i])); return dmax; }
Real RobotIKError(const RobotKinematics3D& robot,const IKGoal& goal) { int m=IKGoal::NumDims(goal.posConstraint); int n=IKGoal::NumDims(goal.rotConstraint); Real poserr[3],orierr[3]; if(goal.destLink < 0) goal.GetError(robot.links[goal.link].T_World,poserr,orierr); else { RigidTransform Trel; Trel.mulInverseB(robot.links[goal.link].T_World,robot.links[goal.destLink].T_World); goal.GetError(Trel,poserr,orierr); } Real emax=0.0; for(int i=0;i<m;i++) emax = Max(emax,Abs(poserr[i])); for(int i=0;i<n;i++) emax = Max(emax,Abs(orierr[i])); return emax; }
void OrientedSupportPolygon::GetXYSlice(Real height,UnboundedPolytope2D& poly) const { //transform SP space to original space RigidTransform Tinv; Tinv.setInverse(T); Vector3 x,y,z; Tinv.R.get(x,y,z); Matrix2 R2; R2(0,0) = T.R(0,0); R2(0,1) = T.R(0,1); R2(1,0) = T.R(1,0); R2(1,1) = T.R(1,1); Real det=R2.determinant(); Assert(Abs(det) > Epsilon); //otherwise the slice is infinite... Assert(Abs(z.z) > Epsilon); //otherwise the slice is infinite... bool flip = (det < 0.0); //the plane is flipped poly.vertices.resize(sp.vertices.size()); for(size_t i=0;i<sp.vertices.size();i++) { const PointRay2D& v=sp.vertices[i]; Real u; //let p = [v.x,v.y,u] (u unknown), q be the transformed point if(v.isRay) { //pick u s.t. p = T*[q.x,q.y,0] //parallel to xy plane u = (-x.z*v.x - y.z*v.y)/z.z; poly.vertices[i].isRay = true; poly.vertices[i].x = x.x*v.x + y.x*v.y + u*z.x; poly.vertices[i].y = x.y*v.x + y.y*v.y + u*z.y; } else { //pick u s.t. p = T*[q.x,q.y,height] u = (height - x.z*v.x - y.z*v.y - Tinv.t.z)/z.z; poly.vertices[i].isRay = false; poly.vertices[i].x = x.x*v.x + y.x*v.y + u*z.x + Tinv.t.x; poly.vertices[i].y = x.y*v.x + y.y*v.y + u*z.y + Tinv.t.y; } } if(flip) //maintain CCW ordering reverse(poly.vertices.begin(),poly.vertices.end()); //laziness poly.CalcPlanes(); }
/** Two particles */ void test_2particles( Real stiffness, Real dampingRatio, Real restLength, Vec3 x0, Vec3 v0, Vec3 x1, Vec3 v1, Vec3 f0) { // set position and velocity vectors, using DataTypes::set to cope with tests in dimension 2 VecCoord x(2); DataTypes::set( x[0], x0[0],x0[1],x0[2]); DataTypes::set( x[1], x1[0],x1[1],x1[2]); VecDeriv v(2); DataTypes::set( v[0], v0[0],v0[1],v0[2]); DataTypes::set( v[1], v1[0],v1[1],v1[2]); VecDeriv f(2); DataTypes::set( f[0], f0[0], f0[1], f0[2]); DataTypes::set( f[1], -f0[0],-f0[1],-f0[2]); // randomly rotate and translate the scene /// @todo functions to apply the transform to VecCoord and VecDeriv RigidTransform<DataTypes> transform; transform.randomize(1.1); transform.projectPoint( x[0]); transform.projectVector(v[0]); transform.projectPoint( x[1]); transform.projectVector(v[1]); transform.projectVector(f[0]); transform.projectVector(f[1]); // tune the force field this->force->addSpring(0,1,stiffness,dampingRatio,restLength); // cout<<"test_2particles, x = " << x << endl; // cout<<" v = " << v << endl; // cout<<" f = " << f << endl; // and run the test this->run_test( x, v, f ); }