Skip to content

Commit

Permalink
Remove unnatural vibration when grounded, by @lovettchris , PR micros…
Browse files Browse the repository at this point in the history 
…oft#670
  • Loading branch information
@sytelus
sytelus committed Dec 9, 2017
1 parent be9c0e3 commit 261a76f
Show file tree
Hide file tree
Showing 11 changed files with 281 additions and 108 deletions.
4 changes: 3 additions & 1 deletion .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -320,4 +320,6 @@ deps/
/PythonClient/cloud.txt
/Unreal/Plugins/AirSim/Content/VehicleAdv/SUV/
/suv_download_tmp
car_assets.zip
car_assets.zip
/PythonClient/cloud.txt
__pycache__/
152 changes: 100 additions & 52 deletions AirLib/include/physics/FastPhysicsEngine.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,17 +78,19 @@ class FastPhysicsEngine : public PhysicsEngineBase {

//first compute the response as if there was no collision
//this is necessory to take in to account forces and torques generated by body
getNextKinematicsNoCollision(dt, body, current, next, next_wrench);
getNextKinematicsNoCollision(dt, body, current, next, next_wrench, grounded_);

//if there is collision, see if we need collision response
const CollisionInfo collision_info = body.getCollisionInfo();
CollisionResponseInfo& collision_response_info = body.getCollisionResponseInfo();
//if collision was already responsed then do not respond to it until we get updated information
if (collision_info.has_collided && collision_response_info.collision_time_stamp != collision_info.time_stamp) {
bool is_collision_response = getNextKinematicsOnCollision(dt, collision_info, body,
current, next, next_wrench, enable_ground_lock_);
current, next, next_wrench, enable_ground_lock_, grounded_);
updateCollisionResponseInfo(collision_info, next, is_collision_response, collision_response_info);
//throttledLogOutput("*** has collision", 0.1);
}
//else throttledLogOutput("*** no collision", 0.1);

//Utils::log(Utils::stringf("T-VEL %s %" PRIu64 ": ",
// VectorMath::toString(next.twist.linear).c_str(), clock()->getStepCount()));
Expand All @@ -98,7 +100,8 @@ class FastPhysicsEngine : public PhysicsEngineBase {
body.kinematicsUpdated();
}

static void updateCollisionResponseInfo(const CollisionInfo& collision_info, const Kinematics::State& next, bool is_collision_response, CollisionResponseInfo& collision_response_info)
static void updateCollisionResponseInfo(const CollisionInfo& collision_info, const Kinematics::State& next,
bool is_collision_response, CollisionResponseInfo& collision_response_info)
{
collision_response_info.collision_time_stamp = collision_info.time_stamp;
++collision_response_info.collision_count_raw;
Expand All @@ -111,7 +114,7 @@ class FastPhysicsEngine : public PhysicsEngineBase {

//return value indicates if collision response was generated
static bool getNextKinematicsOnCollision(TTimeDelta dt, const CollisionInfo& collision_info, const PhysicsBody& body,
const Kinematics::State& current, Kinematics::State& next, Wrench& next_wrench, bool enable_ground_lock)
const Kinematics::State& current, Kinematics::State& next, Wrench& next_wrench, bool enable_ground_lock, bool& grounded)
{
/************************* Collision response ************************/
const real_T dt_real = static_cast<real_T>(dt);
Expand All @@ -133,7 +136,7 @@ class FastPhysicsEngine : public PhysicsEngineBase {
//see if impact is straight at body's surface (assuming its box)
const Vector3r normal_body = VectorMath::transformToBodyFrame(collision_info.normal, current.pose.orientation);
const bool is_ground_normal = Utils::isApproximatelyEqual(std::abs(normal_body.z()), 1.0f, kAxisTolerance);
bool ground_lock = false;
bool ground_collision = false;
if (is_ground_normal
|| Utils::isApproximatelyEqual(std::abs(normal_body.x()), 1.0f, kAxisTolerance)
|| Utils::isApproximatelyEqual(std::abs(normal_body.y()), 1.0f, kAxisTolerance)
Expand All @@ -142,12 +145,12 @@ class FastPhysicsEngine : public PhysicsEngineBase {
r = Vector3r::Zero();

//we have collided with ground straight on, we will fix orientation later
ground_lock = is_ground_normal;
ground_collision = is_ground_normal;
}

//velocity at contact point
const Vector3r vcur_avg_body = VectorMath::transformToBodyFrame(vcur_avg, current.pose.orientation);
const Vector3r contact_vel_body = vcur_avg_body + angular_avg.cross(r);
const Vector3r contact_vel_body = vcur_avg_body + angular_avg.cross(r);

/*
GafferOnGames - Collision response with columb friction
Expand Down Expand Up @@ -189,25 +192,44 @@ class FastPhysicsEngine : public PhysicsEngineBase {
next.accelerations.angular = Vector3r::Zero();

next.pose = current.pose;
if (enable_ground_lock && ground_lock) {
if (enable_ground_lock && ground_collision) {
float pitch, roll, yaw;
VectorMath::toEulerianAngle(next.pose.orientation, pitch, roll, yaw);
pitch = roll = 0;
next.pose.orientation = VectorMath::toQuaternion(pitch, roll, yaw);

//there is a lot of random angular velocity when vehicle is on the ground
next.twist.angular = Vector3r::Zero();
}
//else keep the orientation
next.pose.position = collision_info.position + (collision_info.normal * collision_info.penetration_depth) + next.twist.linear * (dt_real * kCollisionResponseCycles);

// also eliminate any linear velocity due to twist - since we are sitting on the ground there shouldn't be any.
next.twist.linear = Vector3r::Zero();
next.pose.position = collision_info.position;
grounded = true;
//throttledLogOutput("*** Triggering ground lock", 0.1);
}
else
{
//else keep the orientation
next.pose.position = collision_info.position + (collision_info.normal * collision_info.penetration_depth) + next.twist.linear * (dt_real * kCollisionResponseCycles);
}
next_wrench = Wrench::zero();

//Utils::log(Utils::stringf("*** C-VEL %s: ", VectorMath::toString(next.twist.linear).c_str()));

return true;
}

void throttledLogOutput(const std::string& msg, double seconds)
{
TTimeDelta dt = clock()->elapsedSince(last_message_time);
const real_T dt_real = static_cast<real_T>(dt);
if (dt_real > seconds)
{
Utils::log(msg);
last_message_time = clock()->nowNanos();
}
}

//bool getNextKinematicsOnGround(TTimeDelta dt, const PhysicsBody& body, const Kinematics::State& current, Kinematics::State& next, Wrench& next_wrench)
//{
// /************************* reset state if we have hit the ground ************************/
Expand Down Expand Up @@ -292,59 +314,84 @@ class FastPhysicsEngine : public PhysicsEngineBase {
return wrench;
}

static void getNextKinematicsNoCollision(TTimeDelta dt, const PhysicsBody& body, const Kinematics::State& current, Kinematics::State& next, Wrench& next_wrench)
static void getNextKinematicsNoCollision(TTimeDelta dt, const PhysicsBody& body, const Kinematics::State& current,
Kinematics::State& next, Wrench& next_wrench, bool& grounded)
{
const real_T dt_real = static_cast<real_T>(dt);

Vector3r avg_linear = Vector3r::Zero();
Vector3r avg_angular = Vector3r::Zero();

/************************* Get force and torque acting on body ************************/
//set wrench sum to zero
const Wrench body_wrench = getBodyWrench(body, current.pose.orientation);

//add linear drag due to velocity we had since last dt seconds
//drag vector magnitude is proportional to v^2, direction opposite of velocity
//total drag is b*v + c*v*v but we ignore the first term as b << c (pg 44, Classical Mechanics, John Taylor)
//To find the drag force, we find the magnitude in the body frame and unit vector direction in world frame
const Vector3r avg_linear = current.twist.linear + current.accelerations.linear * (0.5f * dt_real);
const Vector3r avg_angular = current.twist.angular + current.accelerations.angular * (0.5f * dt_real);
const Wrench drag_wrench = getDragWrench(body, current.pose.orientation, avg_linear, avg_angular);

next_wrench = body_wrench + drag_wrench;

//Utils::log(Utils::stringf("B-WRN %s: ", VectorMath::toString(body_wrench.force).c_str()));
//Utils::log(Utils::stringf("D-WRN %s: ", VectorMath::toString(drag_wrench.force).c_str()));

/************************* Update accelerations due to force and torque ************************/
//get new acceleration due to force - we'll use this acceleration in next time step
next.accelerations.linear = (next_wrench.force / body.getMass()) + body.getEnvironment().getState().gravity;
if (grounded) {
// make it stick to the ground until we see significant body wrench forces.
float normalizedForce = body_wrench.force.squaredNorm();
if (normalizedForce > kSurfaceTension)
{
//throttledLogOutput("*** Losing ground lock due to body_wrench " + VectorMath::toString(body_wrench.force), 0.1);
grounded = false;
}
next_wrench.force = Vector3r::Zero();
next_wrench.torque = Vector3r::Zero();
next.accelerations.linear = Vector3r::Zero();
}
else {
//add linear drag due to velocity we had since last dt seconds
//drag vector magnitude is proportional to v^2, direction opposite of velocity
//total drag is b*v + c*v*v but we ignore the first term as b << c (pg 44, Classical Mechanics, John Taylor)
//To find the drag force, we find the magnitude in the body frame and unit vector direction in world frame
avg_linear = current.twist.linear + current.accelerations.linear * (0.5f * dt_real);
avg_angular = current.twist.angular + current.accelerations.angular * (0.5f * dt_real);
const Wrench drag_wrench = getDragWrench(body, current.pose.orientation, avg_linear, avg_angular);

//get new angular acceleration
//Euler's rotation equation: https://en.wikipedia.org/wiki/Euler's_equations_(body_dynamics)
//we will use torque to find out the angular acceleration
//angular momentum L = I * omega
const Vector3r angular_momentum = body.getInertia() * avg_angular;
const Vector3r angular_momentum_rate = next_wrench.torque - avg_angular.cross(angular_momentum);
//new angular acceleration - we'll use this acceleration in next time step
next.accelerations.angular = body.getInertiaInv() * angular_momentum_rate;
next_wrench = body_wrench + drag_wrench;

//Utils::log(Utils::stringf("B-WRN %s: ", VectorMath::toString(body_wrench.force).c_str()));
//Utils::log(Utils::stringf("D-WRN %s: ", VectorMath::toString(drag_wrench.force).c_str()));

/************************* Update accelerations due to force and torque ************************/
//get new acceleration due to force - we'll use this acceleration in next time step

/************************* Update pose and twist after dt ************************/
//Verlet integration: http://www.physics.udel.edu/~bnikolic/teaching/phys660/numerical_ode/node5.html
next.twist.linear = current.twist.linear + (current.accelerations.linear + next.accelerations.linear) * (0.5f * dt_real);
next.twist.angular = current.twist.angular + (current.accelerations.angular + next.accelerations.angular) * (0.5f * dt_real);
next.accelerations.linear = (next_wrench.force / body.getMass()) + body.getEnvironment().getState().gravity;
}

//if controller has bug, velocities can increase idenfinitely
//so we need to clip this or everything will turn in to infinity/nans

if (next.twist.linear.squaredNorm() > EarthUtils::SpeedOfLight * EarthUtils::SpeedOfLight) { //speed of light
next.twist.linear /= (next.twist.linear.norm() / EarthUtils::SpeedOfLight);
next.accelerations.linear = Vector3r::Zero();
}
//
//for disc of 1m radius which angular velocity translates to speed of light on tangent?
if (next.twist.angular.squaredNorm() > EarthUtils::SpeedOfLight * EarthUtils::SpeedOfLight) { //speed of light
next.twist.angular /= (next.twist.angular.norm() / EarthUtils::SpeedOfLight);
if (grounded) {
// this stops vehicle from vibrating while it is on the ground doing nothing.
next.accelerations.angular = Vector3r::Zero();
next.twist.linear = Vector3r::Zero();
next.twist.angular = Vector3r::Zero();
} else {
//get new angular acceleration
//Euler's rotation equation: https://en.wikipedia.org/wiki/Euler's_equations_(body_dynamics)
//we will use torque to find out the angular acceleration
//angular momentum L = I * omega
const Vector3r angular_momentum = body.getInertia() * avg_angular;
const Vector3r angular_momentum_rate = next_wrench.torque - avg_angular.cross(angular_momentum);
//new angular acceleration - we'll use this acceleration in next time step
next.accelerations.angular = body.getInertiaInv() * angular_momentum_rate;

/************************* Update pose and twist after dt ************************/
//Verlet integration: http://www.physics.udel.edu/~bnikolic/teaching/phys660/numerical_ode/node5.html
next.twist.linear = current.twist.linear + (current.accelerations.linear + next.accelerations.linear) * (0.5f * dt_real);
next.twist.angular = current.twist.angular + (current.accelerations.angular + next.accelerations.angular) * (0.5f * dt_real);

//if controller has bug, velocities can increase idenfinitely
//so we need to clip this or everything will turn in to infinity/nans

if (next.twist.linear.squaredNorm() > EarthUtils::SpeedOfLight * EarthUtils::SpeedOfLight) { //speed of light
next.twist.linear /= (next.twist.linear.norm() / EarthUtils::SpeedOfLight);
next.accelerations.linear = Vector3r::Zero();
}
//
//for disc of 1m radius which angular velocity translates to speed of light on tangent?
if (next.twist.angular.squaredNorm() > EarthUtils::SpeedOfLight * EarthUtils::SpeedOfLight) { //speed of light
next.twist.angular /= (next.twist.angular.norm() / EarthUtils::SpeedOfLight);
next.accelerations.angular = Vector3r::Zero();
}
}

computeNextPose(dt, current.pose, avg_linear, avg_angular, next);
Expand Down Expand Up @@ -399,11 +446,12 @@ class FastPhysicsEngine : public PhysicsEngineBase {
static constexpr float kAxisTolerance = 0.25f;
static constexpr float kRestingVelocityMax = 0.1f;
static constexpr float kDragMinVelocity = 0.1f;
static constexpr float kSurfaceTension = 1.0f;

std::stringstream debug_string_;
int grounded_;
bool grounded_ = false;
bool enable_ground_lock_;

TTimePoint last_message_time;
};

}} //namespace
Expand Down
Loading

0 comments on commit 261a76f

Please sign in to comment.