int gradientFunc(const Vector& parameters,
bool new_parameters,
Vector& gradient) const override
{ SimTK_ASSERT2_ALWAYS(gradient.size() == getNumFreeQs(),
"AssemblySystem::gradientFunc(): expected gradient vector of"
" size %d but got %d; this is likely a problem with the optimizer"
" which is required to allocate the right amount of space.",
getNumFreeQs(), gradient.size());
++nEvalGradient;
if (new_parameters)
setInternalStateFromFreeQs(parameters);
for (unsigned i=0; i < assembler.reporters.size(); ++i)
assembler.reporters[i]->handleEvent(getInternalState());
// This will record the indices of any goals we encounter that can't
// provide their own gradients; we'll handle them all together at
// the end.
Array_<AssemblyConditionIndex> needNumericalGradient;
gradient = 0;
Vector tmpGradient(gradient.size());
for (unsigned i=0; i < assembler.goals.size(); ++i) {
AssemblyConditionIndex goalIx = assembler.goals[i];
const AssemblyCondition& cond = *assembler.conditions[goalIx];
const int stat = (assembler.forceNumericalGradient
? -1
: cond.calcGoalGradient(getInternalState(),
tmpGradient));
if (stat == -1) {
needNumericalGradient.push_back(goalIx);
continue;
}
if (stat != 0)
return stat;
gradient += assembler.weights[goalIx] * tmpGradient;
}
if (!needNumericalGradient.empty()) {
//cout << "Need numerical gradient for "
// << needNumericalGradient.size() << " goals." << endl;
NumGradientFunc numGoals(assembler, needNumericalGradient);
// Essential to use central difference here so that the
// approximate gradient is actually zero at the optimum
// solution, otherwise IpOpt won't converge.
Differentiator gradNumGoals
(numGoals,Differentiator::CentralDifference);
// weights are already included here
gradient += gradNumGoals.calcGradient(getFreeQsFromInternalState());
nEvalObjective += gradNumGoals.getNumCallsToUserFunction();
}
//cout << "Grad=" << gradient << endl;
return 0;
}
//------------------------------------------------------------------------------
// REACHING AND GRAVITY COMPENSATION :: CALC DECORATIVE GEOMETRY
//------------------------------------------------------------------------------
void ReachingAndGravityCompensation::
calcDecorativeGeometryAndAppend(const State & state, Stage stage,
Array_<DecorativeGeometry>& geometry) const
{
if (stage != Stage::Position) return;
const Vec3 targetPos = m_modelTasks.getTarget();
geometry.push_back(DecorativeSphere(0.02)
.setTransform(targetPos)
.setColor(m_targetColor));
geometry.push_back(DecorativeText("Target: " +
String(targetPos[0])+","+String(targetPos[1])+","+String(targetPos[2]))
.setIsScreenText(true));
const MobilizedBody& ee = m_realRobot.getEndEffectorBody();
Vec3 taskPosInGround = ee.findStationLocationInGround(state,
m_realRobot.getEndEffectorStation());
geometry.push_back(DecorativePoint(taskPosInGround)
.setColor(Green).setLineThickness(3));
geometry.push_back(DecorativeText(String("TOGGLES: [t]Task point ")
+ (m_modelTasks.isTaskPointFollowingOn() ? "ON" : "OFF")
+ "...[g]Gravity comp "
+ (m_modelTasks.isGravityCompensationOn() ? "ON" : "OFF")
+ "...[p]Posture control "
+ (m_modelTasks.isPoseControlOn() ? "ON" : "OFF")
+ "...[e]End effector sensor "
+ (m_modelTasks.isEndEffectorSensingOn() ? "ON" : "OFF")
)
.setIsScreenText(true));
}
//----------------------------- SET UP VISUALIZER ------------------------------
void MyMechanism::setUpVisualizer() {
m_viz.setShutdownWhenDestructed(true) // make sure display window dies
.setBackgroundType(Visualizer::SolidColor); // turn off Ground & Sky
// Add sliders.
m_viz.addSlider("Motor speed", SliderIdMotorSpeed,
-MaxMotorSpeed, MaxMotorSpeed, InitialMotorSpeed)
.addSlider("Torque limit", SliderIdTorqueLimit,
0, MaxTorqueLimit, InitialTorqueLimit)
.addSlider("Tach", SliderIdTach,
-MaxMotorSpeed, MaxMotorSpeed, 0)
.addSlider("Torque", SliderIdTorque,
-InitialTorqueLimit, InitialTorqueLimit, 0);
// Add Run menu.
Array_<std::pair<String,int> > runMenuItems;
runMenuItems.push_back(std::make_pair("Reset", ResetItem));
runMenuItems.push_back(std::make_pair("Quit", QuitItem));
m_viz.addMenu("Run", MenuIdRun, runMenuItems);
// Add per-frame text and geometry.
m_viz.addDecorationGenerator(new ShowStuff(*this));
// Add an input listener so the user can talk to us.
m_userInput = new Visualizer::InputSilo();
m_viz.addInputListener(m_userInput);
}
void PerSubsystemInfo::
popAllocationStackBackToStage(Array_<T>& stack, const Stage& g) {
unsigned newSize = stack.size();
while (newSize > 0 && stack[newSize-1].getAllocationStage() > g)
stack[--newSize].deepDestruct(*m_stateImpl);
stack.resize(newSize);
}
// Create a polygonal mesh for this torus using parameterization as follows:
// u = [0, 2*Pi] traces a circle in the x-y plane with radius torusRadius,
// which is the centroid of the torus. A point P on this circle is
// given by P = torusRadius*~[cos(u) sin(u) 0].
// v = [0, 2*Pi] traces a circle arond the cross-section (or tube) of the
// torus with radius tubeRadius, at a given u. A point Q on this circle
// is given by Q = (torusRadius + tubeRadius*cos(v))*e1 + tubeRadius*(~[0 0 1]*sin(v))
// where e1 = ~[sin(u) cos(u) 0]. The tube circle is in a plane spanned
// by e1 and the z-axis.
void ContactGeometry::Torus::Impl::createPolygonalMesh(PolygonalMesh& mesh) const {
// TODO add resolution argument
const int numSides = 12; //*resolution;
const int numSlices = 36; //*resolution;
// add vertices
for (int i = 0; i < numSlices; ++i) {
Real u = Real((i*2*SimTK_PI)/numSlices);
UnitVec3 e1(std::sin(u), std::cos(u), 0); // torus circle aligned with z-axis (z-axis through hole)
for (int j = 0; j < numSides; ++j) {
Real v = Real((j*2*SimTK_PI)/numSides);
Vec3 vtx = (torusRadius + tubeRadius*std::cos(v))*e1 + tubeRadius*std::sin(v)*Vec3(0,0,1); // use ZAXIS?
mesh.addVertex(vtx);
}
}
// add faces, be careful to wrap indices for the last slice
int numVertices = mesh.getNumVertices();
// cout << "num verts = " << numVertices << endl;
for (int i = 0; i < numVertices; ++i) {
// cout << "v" << i << ": " << mesh.getVertexPosition(i) << endl;
// define counter-clockwise quad faces
Array_<int> faceIndices;
faceIndices.push_back(i); // u_i,v_i
faceIndices.push_back((i+1)%numVertices); // u_i, v_i+1
faceIndices.push_back((i+1+numSides)%numVertices); // u_i+1, v_i+1
faceIndices.push_back((i+numSides)%numVertices); // u_i+1, v_i
mesh.addFace(faceIndices);
}
}
virtual void generateDecorations(const State& state, Array_<DecorativeGeometry>& geometry) override {
const Vec3 frcColors[] = {Red,Orange,Cyan};
const Vec3 momColors[] = {Blue,Green,Purple};
m_system.realize(state, Stage::Velocity);
const int ncont = m_compliant.getNumContactForces(state);
for (int i=0; i < ncont; ++i) {
const ContactForce& force = m_compliant.getContactForce(state,i);
const ContactId id = force.getContactId();
const Vec3& frc = force.getForceOnSurface2()[1];
const Vec3& mom = force.getForceOnSurface2()[0];
Real frcMag = frc.norm(), momMag=mom.norm();
int frcThickness = 1, momThickness = 1;
Real frcScale = ForceScale, momScale = ForceScale;
while (frcMag > 10)
frcThickness++, frcScale /= 10, frcMag /= 10;
while (momMag > 10)
momThickness++, momScale /= 10, momMag /= 10;
DecorativeLine frcLine(force.getContactPoint(),
force.getContactPoint() + frcScale*frc);
DecorativeLine momLine(force.getContactPoint(),
force.getContactPoint() + momScale*mom);
frcLine.setColor(frcColors[id%3]);
momLine.setColor(momColors[id%3]);
frcLine.setLineThickness(2*frcThickness);
momLine.setLineThickness(2*momThickness);
geometry.push_back(frcLine);
geometry.push_back(momLine);
ContactPatch patch;
const bool found = m_compliant.calcContactPatchDetailsById(state,id,patch);
//cout << "patch for id" << id << " found=" << found << endl;
//cout << "resultant=" << patch.getContactForce() << endl;
//cout << "num details=" << patch.getNumDetails() << endl;
for (int i=0; i < patch.getNumDetails(); ++i) {
const ContactDetail& detail = patch.getContactDetail(i);
const Real peakPressure = detail.getPeakPressure();
// Make a black line from the element's contact point in the normal
// direction, with length proportional to log(peak pressure)
// on that element.
DecorativeLine normal(detail.getContactPoint(),
detail.getContactPoint()+ std::log10(peakPressure)
* detail.getContactNormal());
normal.setColor(Black);
geometry.push_back(normal);
// Make a red line that extends from the contact
// point in the direction of the slip velocity, of length 3*slipvel.
DecorativeLine slip(detail.getContactPoint(),
detail.getContactPoint()+3*detail.getSlipVelocity());
slip.setColor(Red);
geometry.push_back(slip);
}
}
}
Real CylinderImplicitFunction::
calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
if (derivComponents.size() == 1 && derivComponents[0] < 2)
return -2*x[derivComponents[0]]/square(ownerp->getRadius());
if (derivComponents.size() == 2 &&
derivComponents[0] == derivComponents[1] &&
derivComponents[0] < 2 )
return -2/square(ownerp->getRadius());
return 0;
}
Real ObservedPointFitter::findBestFit
(const MultibodySystem& system, State& state,
const Array_<MobilizedBodyIndex>& bodyIxs,
const Array_<Array_<Vec3> >& stations,
const Array_<Array_<Vec3> >& targetLocations,
Real tolerance)
{
Array_<Array_<Real> > weights(stations.size());
for (int i = 0; i < (int)stations.size(); ++i)
for (int j = 0; j < (int)stations[i].size(); ++j)
weights[i].push_back(1.0);
return findBestFit(system, state, bodyIxs, stations, targetLocations, weights, tolerance);
}
// We also rummage through the model to find fixed geometry that should be part
// of every frame. The supplied State must be realized through Instance stage.
void ModelVisualizer::collectFixedGeometry(const State& state) const {
// Collect any fixed geometry from the ModelComponents.
Array_<DecorativeGeometry> fixedGeometry;
_model.generateDecorations
(true, _model.getDisplayHints(), state, fixedGeometry);
for (unsigned i=0; i < fixedGeometry.size(); ++i) {
const DecorativeGeometry& dgeo = fixedGeometry[i];
//cout << dgeo.getBodyId() << dgeo.getTransform() << endl;
_viz->addDecoration(MobilizedBodyIndex(dgeo.getBodyId()),
Transform(), dgeo);
}
}
/** Same as above but for a given time series */
void InverseDynamicsSolver::solve(State &s, const FunctionSet &Qs, const Array_<double> ×, Array_<Vector> &genForceTrajectory)
{
int nq = getModel().getNumCoordinates();
int nt = times.size();
//Preallocate if not done already
genForceTrajectory.resize(nt, Vector(nq));
AnalysisSet& analysisSet = const_cast<AnalysisSet&>(getModel().getAnalysisSet());
//fill in results for each time
for(int i=0; i<nt; i++){
genForceTrajectory[i] = solve(s, Qs, times[i]);
analysisSet.step(s, i);
}
}
Real EllipsoidImplicitFunction::
calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
const Vec3& radii = ownerp->getRadii();
if (derivComponents.size() == 1) {
int c = derivComponents[0];
return -2*x[c]/(radii[c]*radii[c]);
}
if ( derivComponents.size() == 2
&& derivComponents[0] == derivComponents[1]) {
int c = derivComponents[0];
return -2/(radii[c]*radii[c]);
}
// A mixed second derivative, or any higher derivative is zero.
return 0;
}
void Constraint::SphereOnPlaneContactImpl::
calcDecorativeGeometryAndAppendVirtual
(const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
{
// We can't generate the artwork until we know the plane frame and the
// sphere center and radius, which might not be until Position stage.
if ( stage == Stage::Position
&& getMyMatterSubsystemRep().getShowDefaultGeometry())
{
const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
const Parameters& params = getParameters(s);
const Transform& X_FP = params.m_X_FP;
const Vec3& p_BO = params.m_p_BO;
const Real r = params.m_radius;
// TODO: should be instance-stage data from State rather than
// topological data.
// This makes z axis point along plane normal
const MobilizedBodyIndex planeMBIx =
getMobilizedBodyIndexOfConstrainedBody(m_planeBody_F);
const MobilizedBodyIndex ballMBIx =
getMobilizedBodyIndexOfConstrainedBody(m_ballBody_B);
if (m_planeHalfWidth > 0) {
// On the inboard body, draw a gray transparent rectangle,
// outlined in black lines.
geom.push_back(DecorativeBrick
(Vec3(m_planeHalfWidth,m_planeHalfWidth,r/10))
.setColor(Gray)
.setRepresentation(DecorativeGeometry::DrawSurface)
.setOpacity(Real(0.3))
.setBodyId(planeMBIx)
.setTransform(X_FP));
geom.push_back(DecorativeFrame(m_planeHalfWidth/5)
.setColor(Gray)
.setBodyId(planeMBIx)
.setTransform(X_FP));
}
// On the ball body draw an orange mesh sphere.
geom.push_back(DecorativeSphere(r)
.setColor(Orange)
.setRepresentation(DecorativeGeometry::DrawWireframe)
.setBodyId(ballMBIx)
.setTransform(p_BO));
}
}
void Assembler::initialize() const {
if (alreadyInitialized)
return;
++nInitializations;
Array_<QIndex> toBeLocked;
reinitializeWithExtraQsLocked(toBeLocked);
alreadyInitialized = true;
return;
/*NOTREACHED*/
// TODO: This currently unused code would allow the Assembler to lock out
// variables that it thinks aren't worth bothering with. Needs real-world
// testing and probably some override options. And should there be a
// desperation mode where all variables are tried if we can't assemble
// with some of them removed?
Vector grad = abs(asmSys->calcCurrentGradient());
Real maxGrad = 0;
for (FreeQIndex fx(0); fx < grad.size(); ++fx)
maxGrad = std::max(maxGrad, grad[fx]);
if (maxGrad == 0) // no q does anything; probably no objective
maxGrad = Infinity; // no q will be kept for gradient purposes
Matrix jac = asmSys->calcCurrentJacobian();
Vector colNorm(getNumFreeQs());
Real maxJac = 0;
for (FreeQIndex fx(0); fx < grad.size(); ++fx) {
colNorm[fx] = jac(fx).norm();
maxJac = std::max(maxJac, colNorm[fx]);
}
if (maxJac == 0) // no q does anything; probably no constraints
maxJac = Infinity; // no q will be kept for Jacobian purposes
const Real QTol = SqrtEps;
const Real minGradAllowed = maxGrad*QTol;
const Real minJacAllowed = maxJac*QTol;
for (FreeQIndex fx(0); fx < grad.size(); ++fx)
if (grad[fx] < minGradAllowed && colNorm[fx] < minJacAllowed)
toBeLocked.push_back(getQIndexOfFreeQ(fx));
if (toBeLocked.size()) {
cout << "Reinitializing with these q's locked:\n";
cout << toBeLocked << endl;
reinitializeWithExtraQsLocked(toBeLocked);
alreadyInitialized = true;
}
}
virtual void generateDecorations(const State& state, Array_<DecorativeGeometry>& geometry) {
const Vec3 frcColors[] = {Red,Orange,Cyan};
const Vec3 momColors[] = {Blue,Green,Purple};
m_system.realize(state, Stage::Velocity);
const int ncont = m_compliant.getNumContactForces(state);
for (int i=0; i < ncont; ++i) {
const ContactForce& force = m_compliant.getContactForce(state,i);
const ContactId id = force.getContactId();
printf("viz contact %d: id=%d\n", i, (int)id);
const Vec3& frc = force.getForceOnSurface2()[1];
const Vec3& mom = force.getForceOnSurface2()[0];
Real frcMag = frc.norm(), momMag=mom.norm();
int frcThickness = 1, momThickness = 1;
Real frcScale = ForceScale, momScale = ForceScale;
while (frcMag > 10)
frcThickness++, frcScale /= 10, frcMag /= 10;
while (momMag > 10)
momThickness++, momScale /= 10, momMag /= 10;
DecorativeLine frcLine(force.getContactPoint(),
force.getContactPoint() + frcScale*frc);
DecorativeLine momLine(force.getContactPoint(),
force.getContactPoint() + momScale*mom);
frcLine.setColor(frcColors[id%3]);
momLine.setColor(momColors[id%3]);
frcLine.setLineThickness(2*frcThickness);
momLine.setLineThickness(2*momThickness);
geometry.push_back(frcLine);
geometry.push_back(momLine);
}
}
void ContactGeometry::TriangleMesh::
findVertexEdges(int vertex, Array_<int>& edges) const {
// Begin at an arbitrary edge which intersects the vertex.
int firstEdge = getImpl().vertices[vertex].firstEdge;
int previousEdge = firstEdge;
int previousFace = getImpl().edges[firstEdge].faces[0];
// Walk around the vertex, using each edge to find the next face and each
// face to find the next edge.
do {
edges.push_back(previousEdge);
const ContactGeometry::TriangleMesh::Impl::Edge&
edge = getImpl().edges[previousEdge];
int nextFace = (edge.faces[0] == previousFace ? edge.faces[1]
: edge.faces[0]);
const ContactGeometry::TriangleMesh::Impl::Face&
face = getImpl().faces[nextFace];
int nextEdge;
if ( face.edges[0] != previousEdge
&& (face.vertices[0] == vertex || face.vertices[1] == vertex))
nextEdge = face.edges[0];
else if ( face.edges[1] != previousEdge
&& (face.vertices[1] == vertex || face.vertices[2] == vertex))
nextEdge = face.edges[1];
else
nextEdge = face.edges[2];
previousEdge = nextEdge;
previousFace = nextFace;
} while (previousEdge != firstEdge);
}
Real calcDerivative(const Array_<int>& derivComponents, const Vector& x) const
{
Real deriv = 0;
assert(3 == x.size());
int derivOrder = derivComponents.size();
assert(1 <= derivOrder);
if (derivOrder == 1)
{
// too clever
// df/dx
// = 2(x-y) + 2(x-z)
// = 2x - 2y + 2x - 2z
// = 4x - 2y - 2z
// = 6x - 2x - 2y - 2z
deriv = 2 * (3 * x[derivComponents[0]] -x[0] -x[1] -x[2]);
}
else if (derivOrder == 2)
{
if (derivComponents[0] == derivComponents[1])
deriv = 4.0; // df/dx^2
else
deriv = -2.0; // df/dxdy
}
else ; // all derivatives higher than two are zero
return deriv;
}
Real calcDerivative(const Array_<int>& derivComponents, const Vector& x) const{
Real deriv = 0;
assert(1 == x.size());
// Derivatives repeat after 4
int derivOrder = derivComponents.size() % 4;
// Derivatives 1, 5, 9, 13, ... are cos()
if ( 1 == derivOrder ) {
deriv = angle_t(amplitude*cos(x[0]*radians - phase));
}
// Derivatives 2, 6, 10, 14, ... are -sin()
else if ( 2 == derivOrder ) {
deriv = angle_t(-amplitude*sin(x[0]*radians - phase));
}
// Derivatives 3, 7, 11, 15, ... are -cos()
else if ( 3 == derivOrder ) {
deriv = angle_t(-amplitude*cos(x[0]*radians - phase));
}
// Derivatives 0, 4, 8, 12, ... are sin()
else if ( 0 == derivOrder ) {
deriv = angle_t(amplitude*sin(x[0]*radians - phase));
}
else assert(false);
return deriv;
}
int ObservedPointFitter::findBodiesForClonedSystem(MobilizedBodyIndex primaryBodyIx, const Array_<int> numStations, const SimbodyMatterSubsystem& matter, const Array_<Array_<MobilizedBodyIndex> > children, Array_<MobilizedBodyIndex>& bodyIxs) {
findUpstreamBodies(primaryBodyIx, numStations, matter, bodyIxs, 5);
int primaryBodyIndex = bodyIxs.size();
int requiredStations = 5;
findDownstreamBodies(primaryBodyIx, numStations, children, bodyIxs, requiredStations);
return primaryBodyIndex;
}
Real TorusImplicitFunction::
calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
// first derivatives
if (derivComponents.size() == 1) {
if (derivComponents[0]<2) {
Real sqrt_xy = std::sqrt(x[0]*x[0] + x[1]*x[1]);
return 2*x[derivComponents[0]]*(ownerp->getTorusRadius() - sqrt_xy)/
(square(ownerp->getTubeRadius())*sqrt_xy);
}
else
return -2*x[2]/square(ownerp->getTubeRadius());
}
// second derivatives
if (derivComponents.size() == 2) {
if (derivComponents[0] < 2) { // fx_ fy_
if (derivComponents[1] < 2) {
Real tubeRadiusSq = square(ownerp->getTubeRadius());
Real xy = x[0]*x[0] + x[1]*x[1];
Real sqrt_xy = std::sqrt(xy);
Real den = tubeRadiusSq*xy*sqrt_xy;
if (derivComponents[0]==derivComponents[1]) { // fxx or fyy
int idx = derivComponents[1]==0; // if 0 then idx=1, if 1 then idx=0
Real num = 2*ownerp->getTorusRadius()*x[idx]*x[idx];
return num/den - 2/tubeRadiusSq;
}
else { // fxy or fyx
return - 2*ownerp->getTorusRadius()*x[0]*x[1]/den;
}
}
else // fxz = fyz = 0
return 0;
}
else { // fz_
if (derivComponents[1] == 2) // fzz
return -2/square(ownerp->getTubeRadius());
else // fzx = fzy = 0
return 0;
}
}
//TODO higher order derivatives
SimTK_ASSERT1_ALWAYS(!"derivative not implemented",
"Implicit Torus implements 1st&2nd derivs only but %d deriv requested.",
derivComponents.size());
return 0;
}
void ContactGeometry::TriangleMesh::Impl::splitObbAxis
(const Array_<int>& parentIndices, Array_<int>& child1Indices,
Array_<int>& child2Indices, int axis)
{ // For each face, find its minimum and maximum extent along the axis.
Vector minExtent(parentIndices.size());
Vector maxExtent(parentIndices.size());
for (int i = 0; i < (int) parentIndices.size(); i++) {
int* vertexIndices = faces[parentIndices[i]].vertices;
Real minVal = vertices[vertexIndices[0]].pos[axis];
Real maxVal = vertices[vertexIndices[0]].pos[axis];
minVal = std::min(minVal, vertices[vertexIndices[1]].pos[axis]);
maxVal = std::max(maxVal, vertices[vertexIndices[1]].pos[axis]);
minExtent[i] = std::min(minVal, vertices[vertexIndices[2]].pos[axis]);
maxExtent[i] = std::max(maxVal, vertices[vertexIndices[2]].pos[axis]);
}
// Select a split point that tries to put as many faces as possible
// entirely on one side or the other.
Real split = (median(minExtent)+median(maxExtent)) / 2;
// Choose a side for each face.
for (int i = 0; i < (int) parentIndices.size(); i++) {
if (maxExtent[i] <= split)
child1Indices.push_back(parentIndices[i]);
else if (minExtent[i] >= split)
child2Indices.push_back(parentIndices[i]);
else if (0.5*(minExtent[i]+maxExtent[i]) <= split)
child1Indices.push_back(parentIndices[i]);
else
child2Indices.push_back(parentIndices[i]);
}
}
void Constraint::LineOnLineContactImpl::
calcDecorativeGeometryAndAppendVirtual
(const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
{
// We can't generate the artwork until we know the lines' placements,
// which might not be until Position stage.
if ( stage != Stage::Position
|| !getMyMatterSubsystemRep().getShowDefaultGeometry())
return;
const Parameters& params = getParameters(s);
const Real hf = params.hf;
const Real hb = params.hb;
const PositionCache& pc = ensurePositionCacheRealized(s);
const Transform& X_AC = pc.X_AC;
const MobilizedBody& mobod_A = getAncestorMobilizedBody();
const Transform& X_GA = mobod_A.getBodyTransform(s);
const Rotation& R_GA = X_GA.R();
const Transform X_GC = X_GA * X_AC;
// Convert interesting stuff from A to G.
const UnitVec3 df_G = R_GA * X_AC.x();
const UnitVec3 db_G = R_GA * pc.db_A;
const Vec3 p_GQf = X_GA * pc.p_AQf;
const Vec3 p_GQb = X_GA * pc.p_AQb;
const Vec3 half_Lf = hf * df_G;
const Vec3 half_Lb = hb * db_G;
const MobilizedBody& bodyF = getMobilizedBodyFromConstrainedBody(m_mobod_F);
const MobilizedBody& bodyB = getMobilizedBodyFromConstrainedBody(m_mobod_B);
const Transform& X_GF = bodyF.getBodyTransform(s);
const Transform& X_GB = bodyB.getBodyTransform(s);
const Transform X_GEf = X_GF * params.X_FEf;
const Transform X_GEb = X_GB * params.X_BEb;
// On body F draw a green line segment around the orange closest point.
geom.push_back(DecorativeLine(p_GQf-half_Lf, p_GQf+half_Lf)
.setColor(Green));
geom.push_back(DecorativeFrame().setTransform(X_GEf)
.setColor(Green*.9).setLineThickness(1).setScale(0.5)); // F color
geom.push_back(DecorativePoint(p_GQf)
.setColor(Orange).setLineThickness(2)); // B color
// On body B draw an orange line segment around the green closest point.
geom.push_back(DecorativeLine(p_GQb-half_Lb, p_GQb+half_Lb)
.setColor(Orange));
geom.push_back(DecorativeFrame().setTransform(X_GEb)
.setColor(Orange*.9).setLineThickness(1).setScale(0.5)); // B color
geom.push_back(DecorativePoint(p_GQb)
.setColor(Green).setLineThickness(2)); // F color
// Show the contact frame in red.
geom.push_back(DecorativeFrame().setTransform(X_GC)
.setColor(Red));
}
void testSpeedSimTKArray() {
Array_<int> v;
v.reserve(Inner);
for (int i=0; i < Outer; ++i) {
v.clear();
for (int i=0; i < Inner; ++i)
v.push_back(i);
}
int sum;
for (int i=0; i < Outer; ++i) {
sum = i;
for (unsigned i=0; i < v.size(); ++i)
sum += v[i];
}
cout << "Array sum=" << sum << endl;
}
void ObservedPointFitter::findDownstreamBodies(MobilizedBodyIndex currentBodyIx, const Array_<int> numStations, const Array_<Array_<MobilizedBodyIndex> > children, Array_<MobilizedBodyIndex>& bodyIxs, int& requiredStations) {
if (numStations[currentBodyIx] == 0 && children[currentBodyIx].empty())
return; // There's no benefit from including this body.
bodyIxs.push_back(currentBodyIx);
requiredStations -= numStations[currentBodyIx];
for (int i = 0; i < (int)children[currentBodyIx].size() && requiredStations > 0; ++i) {
findDownstreamBodies(children[currentBodyIx][i], numStations, children, bodyIxs, requiredStations);
}
}
NumJacobianFunc(Assembler& assembler,
const Array_<AssemblyConditionIndex>& numCons,
const Array_<int>& nErrorEqns,
int totalNEqns)
: Differentiator::JacobianFunction
(totalNEqns, assembler.getNumFreeQs()),
assembler(assembler), numCons(numCons), nEqns(nErrorEqns),
totalNEqns(totalNEqns)
{ assert(numCons.size() == nEqns.size()); }
void PerSubsystemInfo::copyAllocationStackThroughStage
(Array_<T>& stack, const Array_<T>& src, const Stage& g)
{
unsigned nVarsToCopy = src.size(); // assume we'll copy all
while (nVarsToCopy && src[nVarsToCopy-1].getAllocationStage() > g)
--nVarsToCopy;
resizeAllocationStack(stack, nVarsToCopy);
for (unsigned i=0; i < nVarsToCopy; ++i)
stack[i].deepAssign(src[i]);
}
void ObservedPointFitter::findUpstreamBodies(MobilizedBodyIndex currentBodyIx, const Array_<int> numStations, const SimbodyMatterSubsystem& matter, Array_<MobilizedBodyIndex>& bodyIxs, int requiredStations) {
const MobilizedBody& currentBody = matter.getMobilizedBody(currentBodyIx);
if (currentBody.isGround())
return;
MobilizedBodyIndex parentIx = currentBody.getParentMobilizedBody().getMobilizedBodyIndex();
requiredStations -= numStations[parentIx];
if (requiredStations > 0)
findUpstreamBodies(parentIx, numStations, matter, bodyIxs, requiredStations);
bodyIxs.push_back(parentIx);
}
void calcVelocityDotErrors
(const State& s,
const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
const Array_<Real, ConstrainedUIndex>& constrainedUDot,
Array_<Real>& vaerr) const override
{
assert(vaerr.size() == 1);
vaerr[0] = getOneUDot(s, constrainedUDot,
theMobilizer, whichMobility);
}
// One non-holonomic (well, velocity-level) constraint equation.
// verr = u - s
// aerr = udot
//
void calcVelocityErrors
(const State& s,
const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
const Array_<Real, ConstrainedUIndex>& constrainedU,
Array_<Real>& verr) const override
{
assert(verr.size() == 1);
verr[0] = getOneU(s, constrainedU, theMobilizer, whichMobility)
- prescribedSpeed;
}
//------------------------------------------------------------------------------
// CALC TIME OF NEXT SCHEDULED REPORT
//------------------------------------------------------------------------------
void Subsystem::Guts::
calcTimeOfNextScheduledReport(const State& state, Real& tNextReport,
Array_<EventId>& eventIds,
bool includeCurrentTime) const
{
tNextReport = Infinity;
eventIds.clear();
calcTimeOfNextScheduledReportImpl(state, tNextReport, eventIds,
includeCurrentTime);
}
// apply generalized force lambda to the mobility
void addInVelocityConstraintForcesVirtual
(const State& s,
const Array_<Real>& multipliers,
Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
Array_<Real,ConstrainedUIndex>& mobilityForces) const
{
assert(multipliers.size() == 1);
const Real lambda = multipliers[0];
addInOneMobilityForce(s, theMobilizer, whichMobility,
lambda, mobilityForces);
}
