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