示例#1
0
WeldConstraint::WeldConstraint(const std::string &name, OpenSim::Body& body1, SimTK::Transform transformInBody1, 
							   OpenSim::Body& body2, SimTK::Transform transformInBody2) : Constraint()
{
	constructProperties();
	setName(name);
	_body1 = &body1;
	_body2 = &body2;
	set_body_1(body1.getName());
	set_body_2(body2.getName());
	set_location_body_1(transformInBody1.p());
	set_orientation_body_1(transformInBody1.R().convertRotationToBodyFixedXYZ());
	set_location_body_2(transformInBody2.p());
	set_orientation_body_2(transformInBody2.R().convertRotationToBodyFixedXYZ());
}
void testPhysicalOffsetFrameOnPhysicalOffsetFrame()
{
    SimTK::Vec3 tolerance(SimTK::Eps);

    cout << "\nRunning testPhysicalOffsetFrameOnPhysicalOffsetFrame" << endl;
    Model* pendulum = new Model("double_pendulum.osim");

    const OpenSim::Body& rod1 = pendulum->getBodySet().get("rod1");
    
    SimTK::Transform X_RO;
    //offset position by some random vector
    X_RO.setP(SimTK::Vec3(1.2, 2.5, 3.3));
    // rotate the frame 
    X_RO.updR().setRotationToBodyFixedXYZ(SimTK::Vec3(0.33, 0.22, 0.11));
    PhysicalOffsetFrame* offsetFrame = new PhysicalOffsetFrame(rod1, X_RO);
    offsetFrame->setName("first");
    pendulum->addComponent(offsetFrame);

    //connect a second frame to the first PhysicalOffsetFrame 
    PhysicalOffsetFrame* secondFrame = offsetFrame->clone();

    secondFrame->setName("second");
    secondFrame->setParentFrame(*offsetFrame);
    X_RO.setP(SimTK::Vec3(3.3, 2.2, 1.1));
    X_RO.updR().setRotationToBodyFixedXYZ(SimTK::Vec3(1.5, -0.707, 0.5));
    secondFrame->setOffsetTransform(X_RO);
    pendulum->addComponent(secondFrame);

    SimTK::State& s = pendulum->initSystem();

    const Frame& base = secondFrame->findBaseFrame();
    SimTK::Transform XinBase = secondFrame->findTransformInBaseFrame();

    const SimTK::Transform& X_GR = rod1.getTransformInGround(s);
    const SimTK::Transform& X_GO = secondFrame->getTransformInGround(s);

    SimTK::Vec3 angs_known = XinBase.R().convertRotationToBodyFixedXYZ();

    // Compute the offset of these frames in ground
    SimTK::Transform X_RO_2 = ~X_GR*X_GO;
    SimTK::Vec3 angles = X_RO_2.R().convertRotationToBodyFixedXYZ();

    // Offsets should be identical expressed in ground or in the Body
    ASSERT_EQUAL(XinBase.p(), X_RO_2.p(), tolerance,
        __FILE__, __LINE__, 
        "testPhysicalOffsetFrameOnPhysicalOffsetFrame(): incorrect expression of offset in ground.");
    ASSERT_EQUAL(angs_known, angles, tolerance,
        __FILE__, __LINE__, 
        "testPhysicalOffsetFrameOnPhysicalOffsetFrame(): incorrect expression of offset in ground.");

    // make sure that this PhysicalOffsetFrame knows that it is rigidly fixed to the
    // same MobilizedBody as Body rod1
    ASSERT(rod1.getMobilizedBodyIndex() == secondFrame->getMobilizedBodyIndex(),
        __FILE__, __LINE__, 
        "testPhysicalOffsetFrameOnPhysicalOffsetFrame(): incorrect MobilizedBodyIndex");

    // test base Frames are identical
    const Frame& baseRod = rod1.findBaseFrame();
    ASSERT(base == baseRod, __FILE__, __LINE__, 
        "testPhysicalOffsetFrameOnPhysicalOffsetFrame(): incorrect base frame for PhysicalOffsetFrame");
    const Frame& base1 = offsetFrame->findBaseFrame();
    ASSERT(base1 == base, __FILE__, __LINE__,
        "testPhysicalOffsetFrameOnPhysicalOffsetFrame(): incorrect base frames for PhysicalOffsetFrame");
}