コード例 #1
0
// Implement generateDecorations by WrapCylinder to replace the previous out of place implementation
// in ModelVisualizer
void WrapCylinder::generateDecorations(bool fixed, const ModelDisplayHints& hints, const SimTK::State& state,
                                       SimTK::Array_<SimTK::DecorativeGeometry>& appendToThis) const
{

    Super::generateDecorations(fixed, hints, state, appendToThis);
    if (fixed) return;

    if (hints.get_show_wrap_geometry()) {
        const Appearance& defaultAppearance = get_Appearance();
        if (!defaultAppearance.get_visible()) return;
        const Vec3 color = defaultAppearance.get_color();

        SimTK::Transform ztoy;
        // Make transform that takes z axis to y axis due to different
        // assumptions between DecorativeCylinder aligned with y  and
        // WrapCylinder aligned with z
        ztoy.updR().setRotationFromAngleAboutX(SimTK_PI / 2);
        const SimTK::Transform& X_GB = getFrame().getTransformInGround(state);
        SimTK::Transform X_GW = X_GB*getTransform()*ztoy;
        appendToThis.push_back(
            SimTK::DecorativeCylinder(get_radius(),
                                      get_length() / 2)
            .setTransform(X_GW).setResolution(2.0)
            .setColor(color).setOpacity(defaultAppearance.get_opacity())
            .setScale(1).setRepresentation(defaultAppearance.get_representation()));
    }
}
コード例 #2
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());
}
コード例 #3
0
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");
}