/**
* Calculate the wrapping of one line segment over the torus.
*
* @param aPoint1 One end of the line segment
* @param aPoint2 The other end of the line segment
* @param aPathWrap An object holding the parameters for this line/torus pairing
* @param aWrapResult The result of the wrapping (tangent points, etc.)
* @param aFlag A flag for indicating errors, etc.
* @return The status, as a WrapAction enum
*/
int WrapTorus::wrapLine(const SimTK::State& s, SimTK::Vec3& aPoint1, SimTK::Vec3& aPoint2,
const PathWrap& aPathWrap, WrapResult& aWrapResult, bool& aFlag) const
{
int i;
SimTK::Vec3 closestPt;
//bool constrained = (bool) (_wrapSign != 0);
//bool far_side_wrap = false;
aFlag = true;
if (findClosestPoint(_outerRadius, &aPoint1[0], &aPoint2[0], &closestPt[0], &closestPt[1], &closestPt[2], _wrapSign, _wrapAxis) == 0)
return noWrap;
// Now put a cylinder at closestPt and call the cylinder wrap code.
WrapCylinder cyl;//(rot, trans, quadrant, body, radius, length);
SimTK::Vec3 cylXaxis, cylYaxis, cylZaxis; // cylinder axes in torus reference frame
cyl.set_radius(_innerRadius);
cyl.set_length(CYL_LENGTH);
cyl.set_quadrant("+x");
closestPt *= -1;
cylXaxis = closestPt;
Mtx::Normalize(3, cylXaxis, cylXaxis);
cylYaxis[0] = 0.0;
cylYaxis[1] = 0.0;
cylYaxis[2] = -1.0;
Mtx::CrossProduct(cylXaxis, cylYaxis, cylZaxis);
// Note: you don't need to recalculate Y as Z x X because X and Z are always in the XY plane, so Y will remain 0 0 -1.
// cylinderToTorus is the transform from the cylinder to the torus.
// The origin of the cylinder frame in the torus frame is closestPt.
// closestPtCyl is along the X axis of the cylinder since cylXaxis = closestPt.
SimTK::Transform cylinderToTorus(SimTK::Rotation(SimTK::Mat33(cylXaxis[0], cylXaxis[1], cylXaxis[2],
cylYaxis[0], cylYaxis[1], cylYaxis[2], cylZaxis[0], cylZaxis[1], cylZaxis[2])));
SimTK::Vec3 closestPtCyl = cylinderToTorus.shiftFrameStationToBase(closestPt);
cylinderToTorus.setP(closestPtCyl);
Vec3 p1 = cylinderToTorus.shiftFrameStationToBase(aPoint1);
Vec3 p2 = cylinderToTorus.shiftFrameStationToBase(aPoint2);
int return_code = cyl.wrapLine(s, p1, p2, aPathWrap, aWrapResult, aFlag);
if (aFlag == true && return_code > 0) {
aWrapResult.r1 = cylinderToTorus.shiftBaseStationToFrame(aWrapResult.r1);
aWrapResult.r2 = cylinderToTorus.shiftBaseStationToFrame(aWrapResult.r2);
for (i = 0; i < aWrapResult.wrap_pts.getSize(); i++)
aWrapResult.wrap_pts.updElt(i) = cylinderToTorus.shiftBaseStationToFrame(aWrapResult.wrap_pts.get(i));
}
return wrapped;
}
void testClutchedPathSpring()
{
using namespace SimTK;
// start timing
std::clock_t startTime = std::clock();
double mass = 1;
double stiffness = 100;
double dissipation = 0.3;
double start_h = 0.5;
//double ball_radius = 0.25;
//double omega = sqrt(stiffness/mass);
// Setup OpenSim model
Model* model = new Model;
model->setName("ClutchedPathSpringModel");
model->setGravity(gravity_vec);
//OpenSim bodies
const Ground* ground = &model->getGround();
// body that acts as the pulley that the path wraps over
OpenSim::Body* pulleyBody =
new OpenSim::Body("PulleyBody", mass ,Vec3(0), mass*Inertia::brick(0.1, 0.1, 0.1));
// body the path spring is connected to at both ends
OpenSim::Body* block =
new OpenSim::Body("block", mass ,Vec3(0), mass*Inertia::brick(0.2, 0.1, 0.1));
block->attachGeometry(new Brick(Vec3(0.2, 0.1, 0.1)));
block->scale(Vec3(0.2, 0.1, 0.1), false);
//double dh = mass*gravity_vec(1)/stiffness;
WrapCylinder* pulley = new WrapCylinder();
pulley->set_radius(0.1);
pulley->set_length(0.05);
// Add the wrap object to the body, which takes ownership of it
pulleyBody->addWrapObject(pulley);
// Add joints
WeldJoint* weld =
new WeldJoint("weld", *ground, Vec3(0, 1.0, 0), Vec3(0), *pulleyBody, Vec3(0), Vec3(0));
SliderJoint* slider =
new SliderJoint("slider", *ground, Vec3(0), Vec3(0,0,Pi/2),*block, Vec3(0), Vec3(0,0,Pi/2));
double positionRange[2] = {-10, 10};
// Rename coordinates for a slider joint
slider->updCoordinate().setName("block_h");
slider->updCoordinate().setRange(positionRange);
model->addBody(pulleyBody);
model->addJoint(weld);
model->addBody(block);
model->addJoint(slider);
ClutchedPathSpring* spring =
new ClutchedPathSpring("clutch_spring", stiffness, dissipation, 0.01);
spring->updGeometryPath().appendNewPathPoint("origin", *block, Vec3(-0.1, 0.0 ,0.0));
int N = 10;
for(int i=1; i<N; ++i){
double angle = i*Pi/N;
double x = 0.1*cos(angle);
double y = 0.1*sin(angle);
spring->updGeometryPath().appendNewPathPoint("", *pulleyBody, Vec3(-x, y ,0.0));
}
spring->updGeometryPath().appendNewPathPoint("insertion", *block, Vec3(0.1, 0.0 ,0.0));
// BUG in defining wrapping API requires that the Force containing the GeometryPath be
// connected to the model before the wrap can be added
model->addForce(spring);
PrescribedController* controller = new PrescribedController();
controller->addActuator(*spring);
// Control greater than 1 or less than 0 should be treated as 1 and 0 respectively.
double timePts[4] = {0.0, 5.0, 6.0, 10.0};
double clutchOnPts[4] = {1.5, -2.0, 0.5, 0.5};
PiecewiseConstantFunction* controlfunc =
new PiecewiseConstantFunction(4, timePts, clutchOnPts);
controller->prescribeControlForActuator("clutch_spring", controlfunc);
model->addController(controller);
model->print("ClutchedPathSpringModel.osim");
//Test deserialization
delete model;
model = new Model("ClutchedPathSpringModel.osim");
// Create the force reporter
ForceReporter* reporter = new ForceReporter(model);
model->addAnalysis(reporter);
model->setUseVisualizer(false);
SimTK::State& state = model->initSystem();
CoordinateSet& coords = model->updCoordinateSet();
coords[0].setValue(state, start_h);
model->getMultibodySystem().realize(state, Stage::Position );
//==========================================================================
// Compute the force and torque at the specified times.
RungeKuttaMersonIntegrator integrator(model->getMultibodySystem() );
integrator.setAccuracy(integ_accuracy);
Manager manager(*model, integrator);
manager.setWriteToStorage(true);
manager.setInitialTime(0.0);
double final_t = 4.99999;
manager.setFinalTime(final_t);
manager.integrate(state);
// tension is dynamics dependent because controls must be computed
model->getMultibodySystem().realize(state, Stage::Dynamics);
spring = dynamic_cast<ClutchedPathSpring*>(
&model->updForceSet().get("clutch_spring"));
// Now check that the force reported by spring
double model_force = spring->getTension(state);
double stretch0 = spring->getStretch(state);
// the tension should be half the weight of the block
double analytical_force = -0.5*(gravity_vec(1))*mass;
cout << "Tension is: " << model_force << " and should be: " << analytical_force << endl;
// error if the block does not reach equilibrium since spring is clamped
ASSERT_EQUAL(model_force, analytical_force, 10*integ_accuracy);
// unclamp and continue integrating
manager.setInitialTime(final_t);
final_t = 5.99999;
manager.setFinalTime(final_t);
manager.integrate(state);
// tension is dynamics dependent because controls must be computed
model->getMultibodySystem().realize(state, Stage::Dynamics);
// tension should go to zero quickly
model_force = spring->getTension(state);
cout << "Tension is: " << model_force << " and should be: 0.0" << endl;
// is unclamped and block should be in free-fall
ASSERT_EQUAL(model_force, 0.0, 10*integ_accuracy);
// spring is reclamped at 7s so keep integrating
manager.setInitialTime(final_t);
final_t = 10.0;
manager.setFinalTime(final_t);
manager.integrate(state);
// tension is dynamics dependent because controls must be computed
model->getMultibodySystem().realize(state, Stage::Dynamics);
// tension should build to support the block again
model_force = spring->getTension(state);
double stretch1 = spring->getStretch(state);
cout << "Tension is: " << model_force << " and should be: "<< analytical_force << endl;
// is unclamped and block should be in free-fall
ASSERT_EQUAL(model_force, analytical_force, 10*integ_accuracy);
cout << "Steady stretch at control = 1.0 is " << stretch0 << " m." << endl;
cout << "Steady stretch at control = 0.5 is " << stretch1 << " m." << endl;
ASSERT_EQUAL(2*stretch0, stretch1, 10*integ_accuracy);
manager.getStateStorage().print("clutched_path_spring_states.sto");
model->getControllerSet().printControlStorage("clutched_path_spring_controls.sto");
// Save the forces
reporter->getForceStorage().print("clutched_path_spring_forces.mot");
model->disownAllComponents();
cout << " ********** Test clutched spring time = ********** " <<
1.e3*(std::clock()-startTime)/CLOCKS_PER_SEC << "ms\n" << endl;
}
void testPathSpring()
{
using namespace SimTK;
double mass = 1;
double stiffness = 10;
double restlength = 0.5;
double dissipation = 0.1;
double h0 = 0;
double start_h = 0.5;
double ball_radius = 0.25;
double omega = sqrt(stiffness/mass);
double dh = mass*gravity_vec(1)/stiffness;
// Setup OpenSim model
Model *osimModel = new Model;
osimModel->setName("PathSpring");
//OpenSim bodies
OpenSim::Body& ground = osimModel->getGroundBody();
OpenSim::Body pulleyBody("PulleyBody", mass ,Vec3(0), mass*SimTK::Inertia::brick(0.1, 0.1, 0.1));
OpenSim::Body block("block", mass ,Vec3(0), mass*SimTK::Inertia::brick(0.2, 0.1, 0.1));
block.addDisplayGeometry("box.vtp");
block.scale(Vec3(0.2, 0.1, 0.1), false);
WrapCylinder* pulley = new WrapCylinder();
pulley->setRadius(0.1);
pulley->setLength(0.05);
// Add the wrap object to the body, which takes ownership of it
pulleyBody.addWrapObject(pulley);
// Add joints
WeldJoint weld("pulley", ground, Vec3(0, 1.0, 0), Vec3(0), pulleyBody, Vec3(0), Vec3(0));
SliderJoint slider("slider", ground, Vec3(0), Vec3(0,0,Pi/2), block, Vec3(0), Vec3(0,0,Pi/2));
double positionRange[2] = {-10, 10};
// Rename coordinates for a slider joint
CoordinateSet &slider_coords = slider.upd_CoordinateSet();
slider_coords[0].setName("block_h");
slider_coords[0].setRange(positionRange);
slider_coords[0].setMotionType(Coordinate::Translational);
osimModel->addBody(&block);
osimModel->addJoint(&weld);
osimModel->addBody(&pulleyBody);
osimModel->addJoint(&slider);
osimModel->setGravity(gravity_vec);
PathSpring spring("spring", restlength, stiffness, dissipation);
spring.updGeometryPath().appendNewPathPoint("origin", block, Vec3(-0.1, 0.0 ,0.0));
int N = 10;
for(int i=1; i<N; ++i){
double angle = i*Pi/N;
double x = 0.1*cos(angle);
double y = 0.1*sin(angle);
spring.updGeometryPath().appendNewPathPoint("", pulleyBody, Vec3(-x, y ,0.0));
}
spring.updGeometryPath().appendNewPathPoint("insertion", block, Vec3(0.1, 0.0 ,0.0));
// BUG in defining wrapping API requires that the Force containing the GeometryPath be
// connected to the model before the wrap can be added
osimModel->addForce(&spring);
// Create the force reporter
ForceReporter* reporter = new ForceReporter(osimModel);
osimModel->addAnalysis(reporter);
//osimModel->setUseVisualizer(true);
SimTK::State& osim_state = osimModel->initSystem();
slider_coords[0].setValue(osim_state, start_h);
osimModel->getMultibodySystem().realize(osim_state, Stage::Position );
//==========================================================================
// Compute the force and torque at the specified times.
RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() );
integrator.setAccuracy(1e-6);
Manager manager(*osimModel, integrator);
manager.setInitialTime(0.0);
double final_t = 10.0;
manager.setFinalTime(final_t);
manager.integrate(osim_state);
// tension should only be velocity dependent
osimModel->getMultibodySystem().realize(osim_state, Stage::Velocity);
// Now check that the force reported by spring
double model_force = spring.getTension(osim_state);
// get acceleration of the block
osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration);
double hddot = osimModel->getCoordinateSet().get("block_h").getAccelerationValue(osim_state);
// the tension should be half the weight of the block
double analytical_force = -0.5*(gravity_vec(1)-hddot)*mass;
// Save the forces
reporter->getForceStorage().print("path_spring_forces.mot");
// something is wrong if the block does not reach equilibrium
ASSERT_EQUAL(analytical_force, model_force, 1e-3);
// Before exiting lets see if copying the spring works
PathSpring *copyOfSpring = spring.clone();
ASSERT(*copyOfSpring == spring);
osimModel->disownAllComponents();
}
