void STGGeneric::makeChildren(SphereTree *st, int node, int level, const SurfaceRep &surRep) const{
// get the error of the parent
Sphere parS = st->nodes.index(node);
double parErr = eval->evalSphere(parS);
// get minimum bounding sphere for points to give to reducer
Sphere boundingSphere;
SFWhite::makeSphere(&boundingSphere, *surRep.getSurPts());
// generate the set of child spheres
Array<Sphere> initChildren, children;
reducer->getSpheres(&initChildren, st->degree, surRep, &boundingSphere, parErr);
// do sphere refit - local optimisation
if (useRefit){
OUTPUTINFO("Refitting\n");
SOPerSphere perSphere;
perSphere.numIter = 3;
perSphere.eval = eval;
perSphere.optimise(&initChildren, surRep);
}
// apply optimiser if required
if (optimiser && (maxOptLevel < 0 || level <= maxOptLevel))
optimiser->optimise(&initChildren, surRep, -1, &parS, level-1);
// remove redundent spheres
RELargest reLargest;
if (!reLargest.reduceSpheres(&children, initChildren, surRep))
children.clone(initChildren);
int numChildren = children.getSize();
if (numChildren == 0)
return;
// get the points that this node covers
const Array<Surface::Point> *surPts = surRep.getSurPts();
int numPts = surPts->getSize();
// info for areas to be covered by each sphere
Array<Array<Surface::Point> > subPts(numChildren);
Array<bool> covered(numPts);
covered.clear();
// make the divisions between the children
SurfaceDivision surDiv;
surDiv.setup(children, surPts);
// do the children
int firstChild = st->getFirstChild(node);
for (int i = 0; i < numChildren; i++){
// get sphere
Sphere s = children.index(i);
// list the points in the sphere
Array<int> listPoints;
surRep.listContainedPoints(&listPoints, NULL, s, NULL);
int numList = listPoints.getSize();
// filter points
Array<Surface::Point> *filterPts = &subPts.index(i);
filterPts->resize(0);
for (int j = 0; j < numList; j++){
// get point
int pI = listPoints.index(j);
Surface::Point p = surPts->index(pI);
// check if it's in the region
if (surDiv.pointInRegion(p.p, i)){
covered.index(j) = true;
filterPts->addItem() = p;
}
}
}
// count/cover uncovered points
for (int i = 0; i < numPts; i++){
if (!covered.index(i)){
// get the point
Point3D p = surPts->index(i).p;
// find the closest sphere
int closestJ = -1;
float closestD = 0;
for (int j = 0; j < numChildren; j++){
Sphere s = children.index(j);
float d = p.distance(s.c) - s.r;
if (d < closestD){
closestJ = j;
closestD = d;
}
}
subPts.index(closestJ).addItem() = surPts->index(i);
}
}
// store spheres & recurse to children
int childNum = firstChild;
for (int i = 0; i < numChildren; i++){
if (subPts.index(i).getSize() > 1){
// recreate the sphere
Sphere s = children.index(i);
// add sphere to tree
st->nodes.index(childNum).c = s.c;
st->nodes.index(childNum).r = s.r;
if (level < st->levels-1 && numChildren > 1){
const Array<Surface::Point> *pts = &subPts.index(i);
// make cells to have 10 pts each, most will have alot more
int numCells = pts->getSize() / PTS_PER_CELL;
int gridDim = ceil(pow(numCells, 1.0 / 3.0));
OUTPUTINFO("numCells = %d, gridDim = %d\n", numCells, gridDim);
// make children by recursion
SurfaceRep subRep;
subRep.setup(*pts, gridDim);
makeChildren(st, childNum, level+1, subRep);
}
childNum++;
}
}
// NULL out the rest of the spheres
for (int i = childNum; i < st->degree; i++)
st->initNode(firstChild+i, level+1);
}
// generate breadth first
void STGGeneric::constructTree(SphereTree *st) const{
CHECK_DEBUG0(st != NULL && st->degree > 1 && st->levels >= 1);
CHECK_DEBUG0(reducer != NULL);
CHECK_DEBUG0(eval != NULL);
CHECK_DEBUG0(surfacePoints != NULL);
// NULL out the entire tree
st->initNode(0);
// make cells to have 10 pts each, most will have alot more
int numCells = surfacePoints->getSize() / PTS_PER_CELL;
int gridDim = ceil(pow(numCells, 1.0 / 3.0));
OUTPUTINFO("numCells = %d, gridDim = %d\n", numCells, gridDim);
SurfaceRep surRep;
surRep.setup(*surfacePoints, gridDim);
// bounding sphere for root - should use vertices
SFWhite::makeSphere(&st->nodes.index(0), *surfacePoints);
// list of points to be covered by each node
unsigned long start, num;
st->getRow(&start, &num, st->levels-1);
Array<Array<int>/**/> pointsInSpheres;
pointsInSpheres.resize(st->nodes.getSize() - num);
// initialise the list for the first node
int numPts = surfacePoints->getSize();
Array<int> *list0 = &pointsInSpheres.index(0);
list0->resize(numPts);
for (int i = 0; i < numPts; i++)
list0->index(i) = i;
// process the remaining levels
int numLeaves = 0;
for (int level = 0; level < st->levels-1; level++){
// get the positions of the nodes
unsigned long start, num;
st->getRow(&start, &num, level);
// update samples etc. for this level
reducer->setupForLevel(level, st->degree, &surRep);
// get the errors for all the spheres in that level
int numActualSpheres = 0;
double averageError = 0;
Array<double> sphereErrors(num);
for (int i = 0; i < num; i++){
Sphere s = st->nodes.index(start+i);
if (s.r >= 0){
double err = eval->evalSphere(s);
sphereErrors.index(i) = err;
averageError += err;
numActualSpheres++;
}
else
sphereErrors.index(i) = -1;
}
averageError /= numActualSpheres;
if (level != 0 && numActualSpheres <= 1){
numLeaves++;
continue; // there is only one sphere here - will never improve
}
// process each node's to make children
int maxNode = -1;
double maxR = -1;
int levelChildren = 0;
for (int nodeI = 0; nodeI < num; nodeI++){
//OUTPUTINFO("Level = %d, node = %d\n", level, nodeI);
printf("Level = %d, node = %d\n", level, nodeI);
int node = nodeI + start;
if (st->nodes.index(node).r <= 0){
OUTPUTINFO("R = %f\n", st->nodes.index(node).r);
st->initNode(node);
continue;
}
/*
// hack to do largest sphere at each run - gives guide to good params
double r = st->nodes.index(node).r;
if (r > maxR){
maxR = r;
maxNode = node;
}
}
nodeI = maxNode - start;
int node = maxNode;{
// end hack
*/
// make the set of surface poitns to be covered by this sphere
Array<int> *selPtsI = &pointsInSpheres.index(node);
int numSelPts = selPtsI->getSize();
if (numSelPts <= 0)
break;
Array<Surface::Point> selPts(numSelPts);
for (int i = 0; i < numSelPts; i++)
selPts.index(i) = surfacePoints->index(selPtsI->index(i));
// get filter sphere
Sphere s;
SFWhite::makeSphere(&s, selPts);
// make cells to have 10 pts each, most will have alot more
int numCells = numSelPts / PTS_PER_CELL;
int gridDim = ceil(pow(numCells, 1.0 / 3.0));
OUTPUTINFO("%d Points\n", numSelPts);
OUTPUTINFO("numCells = %d, gridDim = %d\n", numCells, gridDim);
// make new SurfaceRepresentation
SurfaceRep subRep;
subRep.setup(selPts, gridDim);
// compute error for this sphere
double err = sphereErrors.index(nodeI);
if (err > averageError)
err = averageError; // improve the bad ones a bit more
// generate the children nodes
Array<Sphere> initChildren, children;
reducer->getSpheres(&initChildren, st->degree, subRep, &s, err);
// apply optimiser if required
if (optimiser && (maxOptLevel < 0 || level <= maxOptLevel)){
printf("RUNNING OPTIMISER...\n");
optimiser->optimise(&initChildren, subRep, -1, &s, level);
printf("DONE OPTIMISING...\n");
}
// do sphere refit - local optimisation
if (useRefit){
OUTPUTINFO("Refitting\n");
SOPerSphere perSphere;
perSphere.numIter = 3;
perSphere.eval = eval;
perSphere.optimise(&initChildren, subRep);
}
// remove redundent spheres
RELargest reLargest;
if (!reLargest.reduceSpheres(&children, initChildren, subRep))
children.clone(initChildren);
// setup the node's sub-division (make the regions to be covered by children)
//subDivs.index(node).setup(children, &selPts);
SurfaceDivision surDiv;
surDiv.setup(children, &selPts);
// list of points that are covered
Array<bool> covered(numSelPts);
covered.clear();
// create the new nodes and their the points to cover
int numChildren = children.getSize();
int firstChild = st->getFirstChild(node);
levelChildren += numChildren;
for (int i = 0; i < numChildren; i++){
int childNum = firstChild + i;
// get sphere
const Sphere& s = children.index(i);
// add sphere to tree
st->nodes.index(childNum).c = s.c;
st->nodes.index(childNum).r = s.r;
if (level < st->levels-2){
// get the points in this sphere
Array<int> pointsInS;
subRep.listContainedPoints(&pointsInS, NULL, s);
int numInS = pointsInS.getSize();
// populate list of points in sphere
Array<int> *pointsToCover = &pointsInSpheres.index(childNum);
pointsToCover->resize(0); // just in case
for (int j = 0; j < numInS; j++){
int pI = pointsInS.index(j);
if (surDiv.pointInRegion(selPts.index(pI).p, i)){
pointsToCover->addItem() = selPtsI->index(pI);
covered.index(pI) = true;
}
}
}
}
// assign uncovered points
if (numChildren > 0 && level < st->levels-2){
for (int i = 0; i < numSelPts; i++){
if (!covered.index(i)){
// get point
const Point3D& pt = selPts.index(i).p;
// find the sphere
int minJ = -1;
float minD = FLT_MAX;
for (int j = 0; j < numChildren; j++){
const Sphere& s = children.index(j);
float d = pt.distance(s.c);// - s.r;
if (d < minD){
minD = d;
minJ = j;
}
}
// add the point to the sphere's list
pointsInSpheres.index(firstChild+minJ).addItem() = selPtsI->index(i);
}
}
}
// save after each child set
//st->saveSphereTree("saved.sph");
}
// save after each level
//st->saveSphereTree("saved.sph");
// see if we need to add another level
if (level == st->levels - 2 && minLeaves > 0 && numLeaves + levelChildren < minLeaves){
// grow the tree
OUTPUTINFO("Growing the tree : %d-->%d\n", st->levels, st->levels+1);
OUTPUTINFO("New Nodes : %d-->", st->nodes.getSize());
st->growTree(st->levels+1);
OUTPUTINFO("%d\n", st->nodes.getSize());
}
}
}
// NOT WORKING YET
double getClosestPoint(ClosestPointInfo *inf, const Point3D &pTest, const Surface &s, const SpacialHash &faceHash, float stopBelow){
// get the dimensions and size of the grid
int nX = faceHash.getDimX();
int nY = faceHash.getDimY();
int nZ = faceHash.getDimZ();
float cellSize = faceHash.getSize();
// work out the MAX ring number (doesn't really need to be small)
int maxRing = nX;
if (nY > maxRing)
maxRing = nY;
if (nZ > maxRing)
maxRing = nZ;
// get the cell
int Cx, Cy, Cz;
faceHash.getBoundedIndices(&Cx, &Cy, &Cz, pTest);
// flags for used triangles
Array<bool> flags(s.triangles.getSize());
flags.clear();
// flags for whether point has been tested
Array<bool> vertexFlags(s.vertices.getSize());
vertexFlags.clear();
// initialise the closest point algorithm
inf->num = 0;
inf->triangle = 0;
inf->type = DIST_TYPE_INVALID;
double minD = REAL_MAX;
double stopBelowSqr = -1;
if (stopBelow > 0)
stopBelowSqr = stopBelow*stopBelow;
// process the first cell
Array<int> *cell = faceHash.getCell(Cx, Cy, Cz);
if (cell->getSize())
getClosestPointINTERNAL(inf, &minD, pTest, s, stopBelowSqr, cell, &flags, &vertexFlags);
#define DOCELL() { \
Array<int> *cell = faceHash.getCell(x, y, z); \
if (cell->getSize() && faceHash.getDistanceSQR(x, y, z, pTest) < minD){ \
getClosestPointINTERNAL(inf, &minD, pTest, s, stopBelowSqr, cell, &flags, &vertexFlags); \
if (minD < stopBelowSqr) \
return sqrt(minD); \
} } \
// process rings
int x, y, z;
for (int ring = 1; ring <= maxRing; ring++){
// check for terminate of ring
double d = (ring-1)*cellSize;
if (d*d > minD)
return sqrt(minD); // done
// get clamped bounds for the ring
int minX = Cx - ring;
if (minX < 0)
minX = 0;
int minY = Cy - ring;
if (minY < 0)
minY = 0;
int minZ = Cz - ring;
if (minZ < 0)
minZ = 0;
int maxX = Cx + ring;
if (maxX >= nX)
maxX = nX-1;
int maxY = Cy + ring;
if (maxY >= nY)
maxY = nY-1;
int maxZ = Cz + ring;
if (maxZ >= nZ)
maxZ = nZ-1;
// top
y = Cy - ring;
if (y >= 0){
for (x = minX; x <= maxX; x++)
for (z = minZ; z <= maxZ; z++)
DOCELL();
}
// bottom
y = Cy + ring;
if (y < nY){
for (x = minX; x <= maxX; x++)
for (z = minZ; z <= maxZ; z++)
DOCELL();
}
// work out the starting points
int localMinY = Cy - ring + 1; // top and bottom already done
if (localMinY < minY)
localMinY = minY;
int localMaxY = Cy + ring - 1;
if (localMaxY > maxY)
localMaxY = maxY;
int localMinZ = Cz - ring;
if (localMinZ < minZ)
localMinZ = minZ;
int localMaxZ = Cz + ring;
if (localMaxZ > maxZ)
localMaxZ = maxZ;
// left
x = Cx - ring;
if (x >= 0){
for (y = localMinY; y <= localMaxY; y++)
for (z = localMinZ; z <= localMaxZ; z++)
DOCELL();
}
// right
x = Cx + ring;
if (x < nX){
for (y = localMinY; y <= localMaxY; y++)
for (z = localMinZ; z <= localMaxZ; z++)
DOCELL();
}
// work out the starting points
int localMinX = Cx - ring + 1; // left and right already done
if (localMinX < minX)
localMinX = minX;
int localMaxX = Cx + ring - 1;
if (localMaxX > maxX)
localMaxX = maxX;
// front
z = Cz - ring;
if (z > 0){
for (x = localMinX; x <= localMaxX; x++)
for (y = localMinY; y <= localMaxY; y++)
DOCELL();
}
// back
z = Cz + ring;
if (z < nZ){
for (x = localMinX; x <= localMaxX; x++)
for (y = localMinY; y <= localMaxY; y++)
DOCELL();
}
}
return sqrt(minD);
}
/**
* Remove points in a signal based on the angle between adjacent segments in
* the signal. The end points of the signal are always retained.
*
* @param aAngle If the angle between two adjacent segments is less than
* aAngle the point in common between the two segments is removed. This
* is evaluate for each point in the signal.
* @param rTime Array of time values. This array is altered.
* @param rSignal Array of signal values. This array is altered.
* @return Number of points removed.
*/
int Signal::
ReduceNumberOfPoints(double aDistance,
Array<double> &rTime,Array<double> &rSignal)
{
// CHECK SIZES
int size = rTime.getSize();
int sizeSignal = rSignal.getSize();
if(size!=sizeSignal) {
cout<<"\n\nSignal.ReduceNumberOfPoints:: quitting. The time and signal ";
cout<<"arrays have different numbers of points.\n\n";
return(0);
}
// CHECK ANGLE
if(aDistance<SimTK::Zero) aDistance = SimTK::Zero;
// APPEND FIRST POINT
Array<double> t(0.0,0,size),s(0.0,0,size);
t.append(rSignal[0]);
s.append(rSignal[0]);
int iLast=0;
// REMOVE POINTS
int i,imid;
SimTK::Vec3 p1(0.0,0.0,0.0);
SimTK::Vec3 p2(0.0,0.0,0.0);
SimTK::Vec3 p3(0.0,0.0,0.0);
SimTK::Vec3 v1(0.0,0.0,0.0);
SimTK::Vec3 v2(0.0,0.0,0.0);
double tmid,mv1,mv2,cos,dsq;
for(i=1;i<(size-1);i++) {
// FIRST POINT
// LAST POINT IN SIMPLIFIED ARRAY
p1[0] = t.getLast();
p1[1] = s.getLast();
// THIRD POINT
p3[0] = rTime[i+1];
p3[1] = rSignal[i+1];
// SECOND POINT
// MIDPOINT
tmid = 0.5*(p3[0]-p1[0]) + p1[0];
imid = rTime.searchBinary(tmid,false,iLast,i+1);
if(imid<=iLast) imid++;
p2[0] = rTime[imid];
p2[1] = rSignal[imid];
v1 = p2 - p1; //Mtx::Subtract(1,3,p2,p1,v1);
v2 = p3 - p1; //Mtx::Subtract(1,3,p3,p1,v2);
mv1 = v1.norm(); //Mtx::Magnitude(3,v1);
mv2 = v2.norm(); //Mtx::Magnitude(3,v2);
cos = (~v1*v2)/(mv1*mv2); //Mtx::DotProduct(3,v1,v2) / (mv1*mv2);
dsq = mv1 * mv1 * (1.0 - cos*cos);
if(dsq < (aDistance*aDistance)) continue;
iLast = i;
t.append(rTime[i]);
s.append(rSignal[i]);
}
// APPEND ENDPOINT
t.append(rTime.getLast());
s.append(rSignal.getLast());
// NUMBER REMOVED
int numberRemoved = rTime.getSize() - t.getSize();
// ALTER ARRAYS
rTime = t;
rSignal = s;
return(numberRemoved);
}
/* Solve for the induced accelerations (udot_f) for a Force in the model
identified by its name. */
const SimTK::Vector& InducedAccelerationsSolver::solve(const SimTK::State& s,
const string& forceName,
bool computeActuatorPotentialOnly,
SimTK::Vector_<SimTK::SpatialVec>* constraintReactions)
{
int nu = _modelCopy.getNumSpeeds();
double aT = s.getTime();
SimTK::State& s_solver = _modelCopy.updWorkingState();
//_modelCopy.initStateWithoutRecreatingSystem(s_solver);
// Just need to set current time and kinematics to determine state of constraints
s_solver.setTime(aT);
s_solver.updQ()=s.getQ();
s_solver.updU()=s.getU();
// Check the external forces and determine if contact constraints should be applied at this time
// and turn constraint on if it should be.
Array<bool> constraintOn = applyContactConstraintAccordingToExternalForces(s_solver);
// Hang on to a state that has the right flags for contact constraints turned on/off
_modelCopy.setPropertiesFromState(s_solver);
// Use this state for the remainder of this step (record)
s_solver = _modelCopy.getMultibodySystem().realizeTopology();
// DO NOT recreate the system, will lose location of constraint
_modelCopy.initStateWithoutRecreatingSystem(s_solver);
//cout << "Solving for contributor: " << _contributors[c] << endl;
// Need to be at the dynamics stage to disable a force
s_solver.setTime(aT);
_modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Dynamics);
if(forceName == "total"){
// Set gravity ON
_modelCopy.getGravityForce().enable(s_solver);
//Use same conditions on constraints
s_solver.updU() = s.getU();
s_solver.updU() = s.getZ();
//Make sure all the actuators are on!
for(int f=0; f<_modelCopy.getActuators().getSize(); f++){
_modelCopy.updActuators().get(f).setDisabled(s_solver, false);
}
// Get to the point where we can evaluate unilateral constraint conditions
_modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Acceleration);
/* *********************************** ERROR CHECKING *******************************
SimTK::Vec3 pcom =_modelCopy.getMultibodySystem().getMatterSubsystem().calcSystemMassCenterLocationInGround(s_solver);
SimTK::Vec3 vcom =_modelCopy.getMultibodySystem().getMatterSubsystem().calcSystemMassCenterVelocityInGround(s_solver);
SimTK::Vec3 acom =_modelCopy.getMultibodySystem().getMatterSubsystem().calcSystemMassCenterAccelerationInGround(s_solver);
SimTK::Matrix M;
_modelCopy.getMultibodySystem().getMatterSubsystem().calcM(s_solver, M);
cout << "mass matrix: " << M << endl;
SimTK::Inertia sysInertia = _modelCopy.getMultibodySystem().getMatterSubsystem().calcSystemCentralInertiaInGround(s_solver);
cout << "system inertia: " << sysInertia << endl;
SimTK::SpatialVec sysMomentum =_modelCopy.getMultibodySystem().getMatterSubsystem().calcSystemMomentumAboutGroundOrigin(s_solver);
cout << "system momentum: " << sysMomentum << endl;
const SimTK::Vector &appliedMobilityForces = _modelCopy.getMultibodySystem().getMobilityForces(s_solver, SimTK::Stage::Dynamics);
appliedMobilityForces.dump("All Applied Mobility Forces");
// Get all applied body forces like those from contact
const SimTK::Vector_<SimTK::SpatialVec>& appliedBodyForces = _modelCopy.getMultibodySystem().getRigidBodyForces(s_solver, SimTK::Stage::Dynamics);
appliedBodyForces.dump("All Applied Body Forces");
SimTK::Vector ucUdot;
SimTK::Vector_<SimTK::SpatialVec> ucA_GB;
_modelCopy.getMultibodySystem().getMatterSubsystem().calcAccelerationIgnoringConstraints(s_solver, appliedMobilityForces, appliedBodyForces, ucUdot, ucA_GB) ;
ucUdot.dump("Udots Ignoring Constraints");
ucA_GB.dump("Body Accelerations");
SimTK::Vector_<SimTK::SpatialVec> constraintBodyForces(_constraintSet.getSize(), SimTK::SpatialVec(SimTK::Vec3(0)));
SimTK::Vector constraintMobilityForces(0);
int nc = _modelCopy.getMultibodySystem().getMatterSubsystem().getNumConstraints();
for (SimTK::ConstraintIndex cx(0); cx < nc; ++cx) {
if (!_modelCopy.getMultibodySystem().getMatterSubsystem().isConstraintDisabled(s_solver, cx)){
cout << "Constraint " << cx << " enabled!" << endl;
}
}
//int nMults = _modelCopy.getMultibodySystem().getMatterSubsystem().getTotalMultAlloc();
for(int i=0; i<constraintOn.getSize(); i++) {
if(constraintOn[i])
_constraintSet[i].calcConstraintForces(s_solver, constraintBodyForces, constraintMobilityForces);
}
constraintBodyForces.dump("Constraint Body Forces");
constraintMobilityForces.dump("Constraint Mobility Forces");
// ******************************* end ERROR CHECKING *******************************/
for(int i=0; i<constraintOn.getSize(); i++) {
_replacementConstraints[i].setDisabled(s_solver, !constraintOn[i]);
// Make sure we stay at Dynamics so each constraint can evaluate its conditions
_modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Acceleration);
}
// This should also push changes to defaults for unilateral conditions
_modelCopy.setPropertiesFromState(s_solver);
}
else if(forceName == "gravity"){
// Set gravity ON
_modelCopy.updForceSubsystem().setForceIsDisabled(s_solver, _modelCopy.getGravityForce().getForceIndex(), false);
// zero velocity
s_solver.setU(SimTK::Vector(nu,0.0));
// disable other forces
for(int f=0; f<_modelCopy.getForceSet().getSize(); f++){
_modelCopy.updForceSet()[f].setDisabled(s_solver, true);
}
}
else if(forceName == "velocity"){
// Set gravity off
_modelCopy.updForceSubsystem().setForceIsDisabled(s_solver, _modelCopy.getGravityForce().getForceIndex(), true);
// non-zero velocity
s_solver.updU() = s.getU();
// zero actuator forces
for(int f=0; f<_modelCopy.getActuators().getSize(); f++){
_modelCopy.updActuators().get(f).setDisabled(s_solver, true);
}
// Set the configuration (gen. coords and speeds) of the model.
_modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Velocity);
}
else{ //The rest are actuators
// Set gravity OFF
_modelCopy.updForceSubsystem().setForceIsDisabled(s_solver, _modelCopy.getGravityForce().getForceIndex(), true);
// zero actuator forces
for(int f=0; f<_modelCopy.getActuators().getSize(); f++){
_modelCopy.updActuators().get(f).setDisabled(s_solver, true);
}
// zero velocity
SimTK::Vector U(nu,0.0);
s_solver.setU(U);
s_solver.updZ() = s.getZ();
// light up the one Force who's contribution we are looking for
int ai = _modelCopy.getForceSet().getIndex(forceName);
if(ai<0){
cout << "Force '"<< forceName << "' not found in model '" <<
_modelCopy.getName() << "'." << endl;
}
Force &force = _modelCopy.getForceSet().get(ai);
force.setDisabled(s_solver, false);
ScalarActuator *actuator = dynamic_cast<ScalarActuator*>(&force);
if(actuator){
if(computeActuatorPotentialOnly){
actuator->overrideActuation(s_solver, true);
actuator->setOverrideActuation(s_solver, 1.0);
}
}
// Set the configuration (gen. coords and speeds) of the model.
_modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Model);
_modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Velocity);
}// End of if to select contributor
// cout << "Constraint 0 is of "<< _constraintSet[0].getConcreteClassName() << " and should be " << constraintOn[0] << " and is actually " << (_constraintSet[0].isDisabled(s_solver) ? "off" : "on") << endl;
// cout << "Constraint 1 is of "<< _constraintSet[1].getConcreteClassName() << " and should be " << constraintOn[1] << " and is actually " << (_constraintSet[1].isDisabled(s_solver) ? "off" : "on") << endl;
// After setting the state of the model and applying forces
// Compute the derivative of the multibody system (speeds and accelerations)
_modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Acceleration);
// Sanity check that constraints hasn't totally changed the configuration of the model
// double error = (s.getQ()-s_solver.getQ()).norm();
// Report reaction forces for debugging
/*
SimTK::Vector_<SimTK::SpatialVec> constraintBodyForces(_constraintSet.getSize());
SimTK::Vector mobilityForces(0);
for(int i=0; i<constraintOn.getSize(); i++) {
if(constraintOn[i])
_constraintSet.get(i).calcConstraintForces(s_solver, constraintBodyForces, mobilityForces);
}*/
return s_solver.getUDot();
}
