void ObservedPointFitter::
createClonedSystem(const MultibodySystem& original, MultibodySystem& copy,
const Array_<MobilizedBodyIndex>& originalBodyIxs,
Array_<MobilizedBodyIndex>& copyBodyIxs,
bool& hasArtificialBaseBody)
{
const SimbodyMatterSubsystem& originalMatter = original.getMatterSubsystem();
SimbodyMatterSubsystem copyMatter(copy);
Body::Rigid body = Body::Rigid(MassProperties(1, Vec3(0), Inertia(1)));
body.addDecoration(Transform(), DecorativeSphere(Real(.1)));
std::map<MobilizedBodyIndex, MobilizedBodyIndex> idMap;
hasArtificialBaseBody = false;
for (int i = 0; i < (int)originalBodyIxs.size(); ++i) {
const MobilizedBody& originalBody = originalMatter.getMobilizedBody(originalBodyIxs[i]);
MobilizedBody* copyBody;
if (i == 0) {
if (originalBody.isGround())
copyBody = ©Matter.Ground();
else {
hasArtificialBaseBody = true; // not using the original joint here
MobilizedBody::Free free(copyMatter.Ground(), body);
copyBody = ©Matter.updMobilizedBody(free.getMobilizedBodyIndex());
}
}
else {
MobilizedBody& parent = copyMatter.updMobilizedBody(idMap[originalBody.getParentMobilizedBody().getMobilizedBodyIndex()]);
copyBody = &originalBody.cloneForNewParent(parent);
}
copyBodyIxs.push_back(copyBody->getMobilizedBodyIndex());
idMap[originalBodyIxs[i]] = copyBody->getMobilizedBodyIndex();
}
copy.realizeTopology();
State& s = copy.updDefaultState();
copyMatter.setUseEulerAngles(s, true);
copy.realizeModel(s);
}
int main() {
// Build a system consisting of a chain of bodies with every possible mobilizer.
MultibodySystem mbs;
SimbodyMatterSubsystem matter(mbs);
Body::Rigid body = Body::Rigid(MassProperties(1, Vec3(0), Inertia(1)));
body.addDecoration(DecorativeSphere(.1));
Random::Uniform random(0.0, 2.0);
MobilizedBody lastBody = MobilizedBody::Pin(matter.Ground(), Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::Slider(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::Universal(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::Cylinder(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::BendStretch(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::Planar(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::Gimbal(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::Ball(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::Translation(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::Free(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::LineOrientation(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::FreeLine(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::Weld(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
lastBody = MobilizedBody::Screw(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())), 0.5);
lastBody = MobilizedBody::Ellipsoid(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
mbs.realizeTopology();
State& s = mbs.updDefaultState();
mbs.realizeModel(s);
// Choose a random initial conformation.
for (int i = 0; i < s.getNQ(); ++i)
s.updQ()[i] = random.getValue();
mbs.realize(s, Stage::Instance);
// The only constraints are the quaternions -- normalize them.
Vector temp;
mbs.project(s, 0.01);
mbs.realize(s, Stage::Position);
// Convert to Euler angles and make sure the positions are all the same.
State euler = s;
matter.convertToEulerAngles(s, euler);
mbs.realize(euler, Stage::Position);
for (int i = 0; i < matter.getNumBodies(); ++i) {
const MobilizedBody& body = matter.getMobilizedBody(MobilizedBodyIndex(i));
Real dist = (body.getBodyOriginLocation(euler)-body.getBodyOriginLocation(s)).norm();
ASSERT(dist < 1e-5);
}
// Now convert back to quaternions and make sure the positions are still the same.
State quaternions = s;
matter.convertToQuaternions(euler, quaternions);
mbs.realize(quaternions, Stage::Position);
for (int i = 0; i < matter.getNumBodies(); ++i) {
const MobilizedBody& body = matter.getMobilizedBody(MobilizedBodyIndex(i));
Real dist = (body.getBodyOriginLocation(quaternions)-body.getBodyOriginLocation(s)).norm();
ASSERT(dist < 1e-5);
}
// Compare the state variables to see if they have been accurately reproduced.
mbs.project(s, 0.01); // Normalize the quaternions
Real diff = std::sqrt((s.getQ()-quaternions.getQ()).normSqr()/s.getNQ());
ASSERT(diff < 1e-5);
std::cout << "Done" << std::endl;
}
void testRiboseMobilizer()
{
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
DecorationSubsystem decorations(system);
matter.setShowDefaultGeometry(false);
// Put some hastily chosen mass there (doesn't help)
Body::Rigid rigidBody;
rigidBody.setDefaultRigidBodyMassProperties(MassProperties(
mass_t(20.0*daltons),
location_t(Vec3(0,0,0)*nanometers),
Inertia(20.0)
));
// One body anchored at C4 atom,
MobilizedBody::Weld c4Body(
matter.updGround(),
Rotation(-120*degrees, XAxis),
rigidBody,
Transform());
// sphere for C4 atom
decorations.addBodyFixedDecoration(
c4Body.getMobilizedBodyIndex(),
Transform(),
DecorativeSphere( length_t(0.5*angstroms) )
);
// sphere for C5 atom
decorations.addBodyFixedDecoration(
c4Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,-1.0,0.5)*angstroms),
DecorativeSphere( length_t(0.5*angstroms) )
);
decorations.addRubberBandLine(
c4Body.getMobilizedBodyIndex(),
Vec3(0),
c4Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,-1.0,0.5)*angstroms),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
// One body anchored at C3 atom -- works
// Pin version
//MobilizedBody::Pin c3Body(
// c4Body,
// Transform(),
// rigidBody,
// Transform(location_t(Vec3(0,0,1.5)*angstroms))
// );
// Function based pin version -- works
//TestPinMobilizer c3Body(
// c4Body,
// Transform(),
// rigidBody,
// Transform(location_t(Vec3(0,0,1.5)*angstroms))
// );
PseudorotationMobilizer c3Body(
c4Body,
Transform(),
rigidBody,
Transform(location_t(Vec3(0,0,1.5)*angstroms)),
angle_t(36.4*degrees), // amplitude
angle_t(-161.8*degrees) // phase
);
// sphere for C3 atom
decorations.addBodyFixedDecoration(
c3Body.getMobilizedBodyIndex(),
Transform(),
DecorativeSphere( length_t(0.5*angstroms) )
);
// sphere for O3 atom
decorations.addBodyFixedDecoration(
c3Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,1.0,-0.5)*angstroms),
DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(1,0,0))
);
decorations.addRubberBandLine(
c3Body.getMobilizedBodyIndex(),
Vec3(0),
c3Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,1.0,-0.5)*angstroms),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
decorations.addRubberBandLine(
c4Body.getMobilizedBodyIndex(),
Vec3(0),
c3Body.getMobilizedBodyIndex(),
Vec3(0),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
PseudorotationMobilizer c2Body(
c3Body,
Rotation( angle_t(-80*degrees), YAxis ),
rigidBody,
Transform(location_t(Vec3(0,0,1.5)*angstroms)),
angle_t(35.8*degrees), // amplitude
angle_t(-91.3*degrees) // phase
);
// sphere for C2 atom
decorations.addBodyFixedDecoration(
c2Body.getMobilizedBodyIndex(),
Transform(),
DecorativeSphere( length_t(0.5*angstroms) )
);
// sphere for O2 atom
decorations.addBodyFixedDecoration(
c2Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,1.0,-0.5)*angstroms),
DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(1,0,0))
);
decorations.addRubberBandLine(
c2Body.getMobilizedBodyIndex(),
Vec3(0),
c2Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,1.0,-0.5)*angstroms),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
decorations.addRubberBandLine(
c3Body.getMobilizedBodyIndex(),
Vec3(0),
c2Body.getMobilizedBodyIndex(),
Vec3(0),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
PseudorotationMobilizer c1Body(
c2Body,
Rotation( angle_t(-80*degrees), YAxis ),
rigidBody,
Transform(location_t(Vec3(0,0,1.5)*angstroms)),
angle_t(37.6*degrees), // amplitude
angle_t(52.8*degrees) // phase
);
// sphere for C1 atom
decorations.addBodyFixedDecoration(
c1Body.getMobilizedBodyIndex(),
Transform(),
DecorativeSphere( length_t(0.5*angstroms) )
);
// sphere for N1 atom
decorations.addBodyFixedDecoration(
c1Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,-1.0,-0.5)*angstroms),
DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(0,0,1))
);
// sphere for O4 atom
decorations.addBodyFixedDecoration(
c1Body.getMobilizedBodyIndex(),
location_t(Vec3(1.0,0,-0.5)*angstroms),
DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(1,0,0))
);
decorations.addRubberBandLine(
c2Body.getMobilizedBodyIndex(),
Vec3(0),
c1Body.getMobilizedBodyIndex(),
Vec3(0),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
decorations.addRubberBandLine(
c1Body.getMobilizedBodyIndex(),
Vec3(0),
c1Body.getMobilizedBodyIndex(),
location_t(Vec3(1.0,0,-0.5)*angstroms),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
decorations.addRubberBandLine(
c1Body.getMobilizedBodyIndex(),
Vec3(0),
c1Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,-1.0,-0.5)*angstroms),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
decorations.addRubberBandLine(
c4Body.getMobilizedBodyIndex(),
Vec3(0),
c1Body.getMobilizedBodyIndex(),
location_t(Vec3(1.0,0,-0.5)*angstroms),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
// Prescribed motion
Constraint::ConstantSpeed(c3Body, 0.5);
// Two constraint way works; one constraint way does not
bool useTwoConstraints = true;
if (useTwoConstraints) {
// Constraints to make three generalized coordinates identical
std::vector<MobilizedBodyIndex> c32bodies(2);
c32bodies[0] = c3Body.getMobilizedBodyIndex();
c32bodies[1] = c2Body.getMobilizedBodyIndex();
std::vector<MobilizerQIndex> coordinates(2, MobilizerQIndex(0));
Constraint::CoordinateCoupler(matter, new DifferenceFunction, c32bodies, coordinates);
std::vector<MobilizedBodyIndex> c21bodies(2);
c21bodies[0] = c2Body.getMobilizedBodyIndex();
c21bodies[1] = c1Body.getMobilizedBodyIndex();
Constraint::CoordinateCoupler(matter, new DifferenceFunction, c21bodies, coordinates);
}
else { // trying to get single constraint way to work
// Try one constraint for all three mobilizers
std::vector<MobilizedBodyIndex> c123Bodies(3);
c123Bodies[0] = c1Body.getMobilizedBodyIndex();
c123Bodies[1] = c2Body.getMobilizedBodyIndex();
c123Bodies[2] = c3Body.getMobilizedBodyIndex();
std::vector<MobilizerQIndex> coords3(3, MobilizerQIndex(0));
Constraint::CoordinateCoupler(matter, new ThreeDifferencesFunction, c123Bodies, coords3);
}
Visualizer viz(system);
viz.setBackgroundType(Visualizer::SolidColor);
system.addEventReporter(new Visualizer::Reporter(viz, 0.10));
system.realizeTopology();
State& state = system.updDefaultState();
// Simulate it.
VerletIntegrator integ(system);
//RungeKuttaMersonIntegrator integ(system);
TimeStepper ts(system, integ);
ts.initialize(state);
ts.stepTo(50.0);
}