Ejemplo n.º 1
0
// Add underly Simbody elements to the System after subcomponents
void BushingForce::
    extendAddToSystemAfterSubcomponents(SimTK::MultibodySystem& system) const
{
    Super::extendAddToSystemAfterSubcomponents(system);

    const SimTK::Vec3& rotStiffness   = get_rotational_stiffness();
    const SimTK::Vec3& transStiffness = get_translational_stiffness();
    const SimTK::Vec3& rotDamping     = get_rotational_damping();
    const SimTK::Vec3& transDamping   = get_translational_damping();

    // get connected frames
    const PhysicalFrame& frame1 = getFrame1();
    const PhysicalFrame& frame2 = getFrame2();

    // Get underlying mobilized bodies
    const SimTK::MobilizedBody& b1 = frame1.getMobilizedBody();
    const SimTK::MobilizedBody& b2 = frame2.getMobilizedBody();

    Vec6 stiffness(rotStiffness[0], rotStiffness[1], rotStiffness[2], 
                   transStiffness[0], transStiffness[1], transStiffness[2]);
    Vec6 damping(rotDamping[0], rotDamping[1], rotDamping[2], 
                 transDamping[0], transDamping[1], transDamping[2]);

    // Now create a Simbody Force::LinearBushing
    SimTK::Force::LinearBushing simtkForce
        (_model->updForceSubsystem(), b1, frame1.findTransformInBaseFrame(),
                                      b2, frame2.findTransformInBaseFrame(),
                                      stiffness, damping );
    
    // Beyond the const Component get the index so we can access the 
    // SimTK::Force later.
    BushingForce* mutableThis = const_cast<BushingForce *>(this);
    mutableThis->_index = simtkForce.getForceIndex();
}
Ejemplo n.º 2
0
void BushingForce::extendAddToSystem(SimTK::MultibodySystem& system) const
{
    Super::extendAddToSystem(system);

    const string&      body1Name            = get_body_1();
    const string&      body2Name            = get_body_2();
    const SimTK::Vec3& locationInBody1      = get_location_body_1();
    const SimTK::Vec3& orientationInBody1   = get_orientation_body_1();
    const SimTK::Vec3& locationInBody2      = get_location_body_2();
    const SimTK::Vec3& orientationInBody2   = get_orientation_body_2();
    const SimTK::Vec3& rotStiffness         = get_rotational_stiffness();
    const SimTK::Vec3& transStiffness       = get_translational_stiffness();
    const SimTK::Vec3& rotDamping           = get_rotational_damping();
    const SimTK::Vec3& transDamping         = get_translational_damping();

    // Get underlying mobilized bodies
    const SimTK::MobilizedBody& b1 = _body1->getMobilizedBody();
    const SimTK::MobilizedBody& b2 = _body2->getMobilizedBody();
    // Build the transforms
    SimTK::Rotation r1; r1.setRotationToBodyFixedXYZ(orientationInBody1);
    SimTK::Rotation r2; r2.setRotationToBodyFixedXYZ(orientationInBody2);
    SimTK::Transform inb1(r1, locationInBody1);
    SimTK::Transform inb2(r2, locationInBody2);

    Vec6 stiffness(rotStiffness[0], rotStiffness[1], rotStiffness[2], 
                   transStiffness[0], transStiffness[1], transStiffness[2]);
    Vec6 damping(rotDamping[0], rotDamping[1], rotDamping[2], 
                 transDamping[0], transDamping[1], transDamping[2]);

    // Now create a Simbody Force::LinearBushing
    SimTK::Force::LinearBushing simtkForce
       (_model->updForceSubsystem(), b1, inb1, b2, inb2, stiffness, damping);
    
    // Beyond the const Component get the index so we can access the 
    // SimTK::Force later.
    BushingForce* mutableThis = const_cast<BushingForce *>(this);
    mutableThis->_index = simtkForce.getForceIndex();
}