예제 #1
0
/**
 *  Test if we have a circle; inertia has been precomputed by caller
 */
double CircleRecognizer::scoreCircle(Stroke* s, Inertia& inertia)
{
	if (inertia.getMass() == 0.0)
	{
		return 0;
	}

	double sum = 0.0;
	double x0 = inertia.centerX();
	double y0 = inertia.centerY();
	double r0 = inertia.rad();

	ArrayIterator<Point> it = s->pointIterator();

	if (!it.hasNext())
	{
		return 0;
	}

	Point p1 = it.next();

	while (it.hasNext())
	{
		Point p2 = it.next();

		double dm = hypot(p2.x - p1.x, p2.y - p1.y);
		double deltar = hypot(p1.x - x0, p1.y - y0) - r0;
		sum += dm * fabs(deltar);

		p1 = p2;
	}

	return sum / (inertia.getMass() * r0);
}
예제 #2
0
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
inline Eigen::Matrix <double, 6, 3> operator* (const Inertia & Y, const ConstraintRotationalSubspace & )
{
    Eigen::Matrix <double, 6, 3> M;
    //    M.block <3,3> (Inertia::LINEAR, 0) = - Y.mass () * skew(Y.lever ());
    M.block <3,3> (Inertia::LINEAR, 0) = alphaSkew ( -Y.mass (),  Y.lever ());
    M.block <3,3> (Inertia::ANGULAR, 0) = (Inertia::Matrix3)(Y.inertia () - Symmetric3::AlphaSkewSquare(Y.mass (), Y.lever ()));
    return M;
}
    /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
    inline Eigen::Matrix<double,6,1>
    operator*( const Inertia& Y,const ConstraintRevoluteUnaligned & cru)
    { 
      /* YS = [ m -mcx ; mcx I-mcxcx ] [ 0 ; w ] = [ mcxw ; Iw -mcxcxw ] */
      const double &m                 = Y.mass();
      const Inertia::Vector3 & c      = Y.lever();
      const Inertia::Symmetric3 & I   = Y.inertia();

      Eigen::Matrix<double,6,1> res;
      res.head<3>() = -m*c.cross(cru.axis);
      res.tail<3>() = I*cru.axis + c.cross(res.head<3>());
      return res;
    }
예제 #4
0
 /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
 inline Eigen::Matrix<double,6,1>
 operator*( const Inertia& Y,const ConstraintPrismatic<2> & )
 { 
   /* Y(:,2) = ( 0,0, 1, y , -x , 0) */
   const double 
   &m = Y.mass(),
   &x = Y.lever()[0],
   &y = Y.lever()[1];
   Eigen::Matrix<double,6,1> res; res << 0.0,0.0,m,
   m*y,
   -m*x,
   0.0;
   return res;
 }
예제 #5
0
 /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
 inline Eigen::Matrix<double,6,1>
 operator*( const Inertia& Y,const ConstraintPrismatic<1> & )
 { 
   /* Y(:,1) = ( 0,1, 0, -z , 0 , x) */
   const double 
   &m = Y.mass(),
   &x = Y.lever()[0],
   &z = Y.lever()[2];
   Eigen::Matrix<double,6,1> res; res << 0.0,m,0.0,
   -m*z,
   0.0,
   m*x ;
   return res;
 }
예제 #6
0
 /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
 inline Eigen::Matrix<double,6,1>
 operator*( const Inertia& Y,const ConstraintPrismatic<0> & )
 { 
   /* Y(:,0) = ( 1,0, 0, 0 , z , -y ) */
   const double 
   &m = Y.mass(),
   &y = Y.lever()[1],
   &z = Y.lever()[2];
   Eigen::Matrix<double,6,1> res; res << m,0.0,0.0,
   0.0,
   m*z,
   -m*y ;
   return res;
 }
예제 #7
0
  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
  Eigen::Matrix <Inertia::Scalar, 6, 3> operator* (const Inertia & Y, const JointPlanar::ConstraintPlanar &)
  {
    Eigen::Matrix <Inertia::Scalar, 6, 3> M;
    const double mass = Y.mass ();
    const Inertia::Vector3 & com = Y.lever ();
    const Symmetric3 & inertia = Y.inertia ();

    M.topLeftCorner <3,3> ().setZero ();
    M.topLeftCorner <2,2> ().diagonal ().fill (mass);

    Inertia::Vector3 mc (mass * com);
    M.rightCols <1> ().head <2> () << mc(1), - mc(0);

    M.bottomLeftCorner <3,2> () << 0., mc(2), - mc(1), 0., mc(1), -mc(0);
    M.rightCols <1> ().tail <3> () = inertia.data ().tail <3> ();
    M.rightCols <1> ().head <2> () = mc.head <2> () * com(2);
    M.rightCols <1> ().tail <1> () = -mc.head <2> ().transpose () * com.head <2> ();

    return M;
  }
예제 #8
0
bool testConstraintRX()
{
  using namespace se3;

  Inertia Y = Inertia::Random();
  JointRX::ConstraintRevolute S;

  std::cout << "Y = \n" << Y.matrix() << std::endl;
  std::cout << "S = \n" << ((ConstraintXd)S).matrix() << std::endl;

  ForceSet F(1); F.block(0,1) = Y*S;
  std::cout << "Y*S = \n" << (Y*S).matrix() << std::endl;
  std::cout << "F=Y*S = \n" << F.matrix() << std::endl;
  assert( F.matrix().isApprox( Y.toMatrix().col(3) ) );

  ForceSet F2( Eigen::Matrix<double,3,9>::Random(),Eigen::Matrix<double,3,9>::Random() );
  Eigen::MatrixXd StF2 = S.transpose()*F2.block(5,3);
  assert( StF2.isApprox( ConstraintXd(S).matrix().transpose()*F2.matrix().block(0,5,6,3) ) );

  return true;
}
예제 #9
0
int main() {
  try
  { // Create the system.

    MultibodySystem         system;
    SimbodyMatterSubsystem  matter(system);
    GeneralForceSubsystem   forces(system);
    Force::Gravity   gravity(forces, matter, UnitVec3(-1,0,0), 9.81);

    ContactTrackerSubsystem  tracker(system);
    CompliantContactSubsystem contactForces(system, tracker);
    contactForces.setTrackDissipatedEnergy(true);
    contactForces.setTransitionVelocity(1e-2); // m/s

    // Ground's normal is +x for this model
    system.setUpDirection(+XAxis);

    // Uncomment this if you want a more elegant movie.
    //matter.setShowDefaultGeometry(false);

    const Real ud = .3; // dynamic
    const Real us = .6; // static
    const Real uv = 0;  // viscous (force/velocity)
    const Real k = 1e8; // pascals
    const Real c = 0.01; // dissipation (1/v)


    // Halfspace default is +x, this one occupies -x instead, so flip.
    const Rotation R_xdown(Pi,ZAxis);

    matter.Ground().updBody().addContactSurface(
        Transform(R_xdown, Vec3(0,0,0)),
        ContactSurface(ContactGeometry::HalfSpace(),
                       ContactMaterial(k,c,us,ud,uv)));


    const Real ellipsoidMass = 1; // kg
    const Vec3 halfDims(2*Cm2m, 20*Cm2m, 3*Cm2m); // m (read in cm)
    const Vec3 comLoc(-1*Cm2m, 0, 0);
    const Inertia centralInertia(Vec3(17,2,16)*CmSq2mSq, Vec3(0,0,.2)*CmSq2mSq); // now kg-m^2
    const Inertia inertia(centralInertia.shiftFromMassCenter(-comLoc, ellipsoidMass)); // in S
    Body::Rigid ellipsoidBody(MassProperties(ellipsoidMass, comLoc, inertia));

    ellipsoidBody.addDecoration(Transform(),
        DecorativeEllipsoid(halfDims).setColor(Cyan)
         //.setOpacity(.5)
         .setResolution(3));
    ellipsoidBody.addContactSurface(Transform(),
        ContactSurface(ContactGeometry::Ellipsoid(halfDims),
                       ContactMaterial(k,c,us,ud,uv))
                       );
    MobilizedBody::Free ellipsoid(matter.Ground(), Transform(Vec3(0,0,0)),
        ellipsoidBody, Transform(Vec3(0)));


    Visualizer viz(system);
    viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
    viz.setMode(Visualizer::RealTime);
    viz.setDesiredFrameRate(FrameRate);
    viz.setCameraClippingPlanes(0.1, 10);

    Visualizer::InputSilo* silo = new Visualizer::InputSilo();
    viz.addInputListener(silo);
    Array_<std::pair<String,int> > runMenuItems;
    runMenuItems.push_back(std::make_pair("Go", GoItem));
    runMenuItems.push_back(std::make_pair("Replay", ReplayItem));
    runMenuItems.push_back(std::make_pair("Quit", QuitItem));
    viz.addMenu("Run", RunMenuId, runMenuItems);

    Array_<std::pair<String,int> > helpMenuItems;
    helpMenuItems.push_back(std::make_pair("TBD - Sorry!", 1));
    viz.addMenu("Help", HelpMenuId, helpMenuItems);

    system.addEventReporter(new MyReporter(system,contactForces,ReportInterval));
    system.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));

    // Check for a Run->Quit menu pick every 1/4 second.
    system.addEventHandler(new UserInputHandler(*silo, .25));

    // Initialize the system and state.

    system.realizeTopology();
    State state = system.getDefaultState();
    matter.setUseEulerAngles(state, true);
    system.realizeModel(state);

    ellipsoid.setQToFitTransform(state, Transform(
        Rotation(BodyRotationSequence,  0  *Deg2Rad, XAxis,
                                        0.5*Deg2Rad, YAxis,
                                       -0.5*Deg2Rad, ZAxis),
        Vec3(2.1*Cm2m, 0, 0)));

    ellipsoid.setUToFitAngularVelocity(state, 2*Vec3(5,0,0)); // rad/s

    viz.report(state);
    printf("Default state\n");

    cout << "\nChoose 'Go' from Run menu to simulate:\n";
    int menuId, item;
    do { silo->waitForMenuPick(menuId, item);
         if (menuId != RunMenuId || item != GoItem)
             cout << "\aDude ... follow instructions!\n";
    } while (menuId != RunMenuId || item != GoItem);



    // Simulate it.

    //ExplicitEulerIntegrator integ(system);
    //CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
    //RungeKuttaFeldbergIntegrator integ(system);
    RungeKuttaMersonIntegrator integ(system);
    //RungeKutta3Integrator integ(system);
    //VerletIntegrator integ(system);
    //integ.setMaximumStepSize(1e-0001);
    integ.setAccuracy(1e-4); // minimum for CPodes
    //integ.setAccuracy(.01);
    TimeStepper ts(system, integ);


    ts.initialize(state);
    double cpuStart = cpuTime();
    double realStart = realTime();

    ts.stepTo(10.0);

    const double timeInSec = realTime() - realStart;
    const int evals = integ.getNumRealizations();
    cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
        timeInSec << "s elapsed for " << ts.getTime() << "s sim (avg step="
        << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
        << (1000*ts.getTime())/evals << "ms/eval\n";
    cout << "  CPU time was " << cpuTime() - cpuStart << "s\n";

    printf("Using Integrator %s at accuracy %g:\n",
        integ.getMethodName(), integ.getAccuracyInUse());
    printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
    printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
    printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());

    viz.dumpStats(std::cout);

    // Add as slider to control playback speed.
    viz.addSlider("Speed", 1, 0, 4, 1);
    viz.setMode(Visualizer::PassThrough);

    silo->clear(); // forget earlier input
    double speed = 1; // will change if slider moves
    while(true) {
        cout << "Choose Run/Replay to see that again ...\n";

        int menuId, item;
        silo->waitForMenuPick(menuId, item);


        if (menuId != RunMenuId) {
            cout << "\aUse the Run menu!\n";
            continue;
        }

        if (item == QuitItem)
            break;
        if (item != ReplayItem) {
            cout << "\aHuh? Try again.\n";
            continue;
        }

        for (double i=0; i < (int)saveEm.size(); i += speed ) {
            int slider; Real newValue;
            if (silo->takeSliderMove(slider,newValue)) {
                speed = newValue;
            }
            viz.report(saveEm[(int)i]);
        }
    }

  } catch (const std::exception& e) {
    std::printf("EXCEPTION THROWN: %s\n", e.what());
    exit(1);

  } catch (...) {
    std::printf("UNKNOWN EXCEPTION THROWN\n");
    exit(1);
  }

    return 0;
}
예제 #10
0
Stroke* CircleRecognizer::recognize(Stroke* stroke)
{
	Inertia s;
	s.calc(stroke->getPoints(), 0, stroke->getPointCount());
	RDEBUG("Mass=%.0f, Center=(%.1f,%.1f), I=(%.0f,%.0f, %.0f), Rad=%.2f, Det=%.4f",
			s.getMass(), s.centerX(), s.centerY(), s.xx(), s.yy(), s.xy(), s.rad(), s.det());

	if (s.det() > CIRCLE_MIN_DET)
	{
		double score = CircleRecognizer::scoreCircle(stroke, s);
		RDEBUG("Circle score: %.2f", score);
		if (score < CIRCLE_MAX_SCORE)
		{
			return CircleRecognizer::makeCircleShape(stroke, s);
		}
	}

	return NULL;
}
  virtual void inertia(Inertia &x, const Joints &q, const ToolMass toolM[2]) {
    x.setZero();
#include "inertia.inc"
  }