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");
}