int main() {
// --------------------- Opensim model.
OpenSim::Model model{};
model.setUseVisualizer(true);
auto slab = new OpenSim::Body{"slab",
1,
SimTK::Vec3{0},
SimTK::Inertia{0}};
auto balljoint = new OpenSim::BallJoint{"balljoint",
model.getGround(),
SimTK::Vec3{0, 1, 0},
SimTK::Vec3{0},
*slab,
SimTK::Vec3{0},
SimTK::Vec3{0}};
OpenSim::Brick brick{};
brick.setFrame(*slab);
brick.set_half_lengths(SimTK::Vec3{0.5, 0.05, 0.25});
slab->addComponent(brick.clone());
model.addBody(slab);
model.addJoint(balljoint);
auto& state = model.initSystem();
model.updMatterSubsystem().setShowDefaultGeometry(false);
SimTK::Visualizer& viz = model.updVisualizer().updSimbodyVisualizer();
viz.setBackgroundType(viz.GroundAndSky);
viz.setShowSimTime(true);
viz.drawFrameNow(state);
// -------------------------- UDP Socket.
constexpr unsigned BUFFSIZE{8192};
auto sock = openUdpSocket();
// ------------------------- Kalman filter.
Kalman kalman_roll{};
Kalman kalman_pitch{};
double oldtimestamp{};
bool firstrow{true};
// ------------------------ Streaming.
while(true) {
char buffer[BUFFSIZE];
auto bytes = recvfrom(sock, buffer, BUFFSIZE, 0, 0, 0);
if(bytes > 0) {
auto data = parseImuData(buffer, BUFFSIZE);
auto& timestamp = std::get<0>(data);
auto& gravity = std::get<1>(data);
auto& omega = std::get<2>(data);
// If omega is (0, 0, 0), skip over because there was no data.
// All three components are never equal except when they are 0.
if(omega[0] == omega[1] && omega[1] == omega[2])
continue;
// Compute change in time and record the timestamp.
auto deltat = timestamp - oldtimestamp;
oldtimestamp = timestamp;
if(firstrow) {
firstrow = false;
continue;
}
auto tilt = computeRollPitch(gravity);
auto roll = radToDeg(tilt.first);
auto pitch = radToDeg(tilt.second);
omega[0] = radToDeg(omega[0]);
omega[1] = radToDeg(omega[1]);
omega[2] = radToDeg(omega[2]);
// Angular velocity about axis y is roll.
// Angular velocity about axis x is pitch.
auto roll_hat = kalman_roll.getAngle( roll, omega[1], deltat);
auto pitch_hat = kalman_pitch.getAngle(pitch, omega[0], deltat);
// Multiplying -1 to roll just for display. This way visualizaiton moves
// like the physical phone.
model.getCoordinateSet()[0].setValue(state, -1 * degToRad( roll_hat));
model.getCoordinateSet()[2].setValue(state, degToRad(pitch_hat));
viz.drawFrameNow(state);
//std::cout << buffer << std::endl;
} else
std::cout << "skipping....." << std::endl;
}
return 0;
}
bool HoverPlaneHelper::findMinimumHoverPlane (const Object & obj, float & roll, float & pitch, const Vector & lookAhead)
{
Object const * motor = obj.getParent ();
if(motor)
{
CollisionProperty const * collision = motor->getCollisionProperty();
if(collision)
{
Footprint const * foot = collision->getFootprint();
if(foot && foot->isOnSolidFloor())
{
Vector const & groundNormal_w = foot->getGroundNormal_w();
const Transform & transform_o2w = obj.getTransform_o2w ();
computeRollPitch (transform_o2w.rotate_p2l (groundNormal_w), roll, pitch);
return true;
}
}
}
// ----------
const Appearance * const app = obj.getAppearance ();
if (!app)
return false;
const BoxExtent * const box = dynamic_cast<const BoxExtent *>(app->getExtent ());
if (!box)
return false;
const Transform & transform_o2w = obj.getTransform_o2w ();
Vector points [10];
if (!transformBoxPoints (*box, transform_o2w, points, lookAhead))
return false;
const TerrainObject * const terrainObject = TerrainObject::getInstance ();
if (!terrainObject)
return false;
Vector normals [10];
Vector avgNormal;
Vector avgLookAhead;
int lookAheadContribCount = 0;
Sphere s = box->getSphere ();
s.setCenter (transform_o2w.getPosition_p ());
float avgDistanceAboveTerrain = 0.0f;
float minDistanceAboveTerrain = 10000.0f;
//- find terrain heights for points
{
const Vector oldCenterPoint = points [0];
float y = 0.0f;
for (int i = 0; i < 10; ++i)
{
if (terrainObject->getHeight (points [i], y, normals [i]))
{
float waterHeight = 0.0f;
const bool isWater = terrainObject->getWaterHeight (points[i], waterHeight);
if(isWater && waterHeight > y)
{
normals[i] = Vector::unitY;
y = waterHeight;
}
if (i >= 5)
{
//-- lookahead point is not in the sphere, let it contribute to the lookahead normal
if (!s.contains (points [i]))
{
points [i].y = y;
avgLookAhead += points [i] - oldCenterPoint;
++lookAheadContribCount;
}
points [i].y = y;
}
else
{
const float distanceAbove = points [i].y - y;
avgDistanceAboveTerrain += distanceAbove;
minDistanceAboveTerrain = std::max (0.0f, std::min (minDistanceAboveTerrain, distanceAbove));
points [i].y = y;
avgNormal += normals [i];
}
}
else
return false;
}
}
avgNormal /= 5.0f;
avgDistanceAboveTerrain /= 5.0f;
computeRollPitch (transform_o2w.rotate_p2l (avgNormal), roll, pitch);
//-- decrease the roll & pitch based on height over terrain
//--
const float extentRadius = s.getRadius ();
avgDistanceAboveTerrain -= extentRadius;
if (avgDistanceAboveTerrain > 0.0f)
{
if (extentRadius > 0.0f)
{
const float relativeDistance = 1.0f + (avgDistanceAboveTerrain / (extentRadius * 0.5f));
roll /= relativeDistance;
pitch /= relativeDistance;
}
}
if (lookAheadContribCount)
{
avgLookAhead /= static_cast<float>(lookAheadContribCount);
Vector vectorToLookAhead = transform_o2w.rotate_p2l (avgLookAhead);
float pitchLookAhead = 0.0f;
if (vectorToLookAhead.normalize ())
pitchLookAhead = vectorToLookAhead.phi ();
else
pitchLookAhead = 0.0f;
if (pitchLookAhead < pitch)
{
pitch = pitchLookAhead;
}
}
return true;
}
