Skip to content

Commit

Permalink
Fix bugs to do with PX4 related to landing. Sim was creating unusual …
Browse files Browse the repository at this point in the history 
…z-acc on landing that confused the PX4 HIL mode landing detector.
  • Loading branch information
@lovettchris
lovettchris committed Apr 2, 2018
1 parent 1c7b250 commit 55e9ecb
Show file tree
Hide file tree
Showing 7 changed files with 41 additions and 49 deletions.
1 change: 1 addition & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@ obj/

# Visual Studio 2015 cache/options directory
.vs/
.vscode/

# MSTest test Results
[Tt]est[Rr]esult*/
Expand Down
74 changes: 33 additions & 41 deletions AirLib/include/physics/FastPhysicsEngine.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,7 @@ class FastPhysicsEngine : public PhysicsEngineBase {
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) {
if (grounded_ || (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_, grounded_);
updateCollisionResponseInfo(collision_info, next, is_collision_response, collision_response_info);
Expand Down Expand Up @@ -137,12 +137,26 @@ class FastPhysicsEngine : public PhysicsEngineBase {
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_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)
const float z_vel = vcur_avg.z();
const bool is_landing = z_vel > std::abs(vcur_avg.x()) && z_vel > std::abs(vcur_avg.y());

double restitution = body.getRestitution();
double friction = body.getFriction();

if (is_ground_normal && is_landing
// So normal_body is the collision normal translated into body coords, why does an x==1 or y==1
// mean we are coliding with the ground???
// || Utils::isApproximatelyEqual(std::abs(normal_body.x()), 1.0f, kAxisTolerance)
// || Utils::isApproximatelyEqual(std::abs(normal_body.y()), 1.0f, kAxisTolerance)
) {
//think of collision occured along the surface, not at point
r = Vector3r::Zero();
// looks like we are coliding with the ground. We don't want the ground to be so bouncy
// so we reduce the coefficient of restitution. 0 means no bounce.
// TODO: it would be better if we did this based on the material we are landing on.
// e.g. grass should be inelastic, but a hard surface like the road should be more bouncy.
restitution = 0;
// crank up friction with the ground so it doesn't try and slide across the ground
// again, this should depend on the type of surface we are landing on.
friction = 1;

//we have collided with ground straight on, we will fix orientation later
ground_collision = is_ground_normal;
Expand All @@ -165,7 +179,7 @@ class FastPhysicsEngine : public PhysicsEngineBase {
(body.getInertiaInv() * r.cross(normal_body))
.cross(r)
.dot(normal_body);
const real_T impulse_mag = -contact_vel_body.dot(normal_body) * (1 + body.getRestitution()) / impulse_mag_denom;
const real_T impulse_mag = -contact_vel_body.dot(normal_body) * (1 + restitution) / impulse_mag_denom;

next.twist.linear = vcur_avg + collision_info.normal * (impulse_mag / body.getMass());
next.twist.angular = angular_avg + r.cross(normal_body) * impulse_mag;
Expand All @@ -178,7 +192,7 @@ class FastPhysicsEngine : public PhysicsEngineBase {
(body.getInertiaInv() * r.cross(contact_tang_unit_body))
.cross(r)
.dot(contact_tang_unit_body);
const real_T friction_mag = -contact_tang_body.norm() * body.getFriction() / friction_mag_denom;
const real_T friction_mag = -contact_tang_body.norm() * friction / friction_mag_denom;

const Vector3r contact_tang_unit = VectorMath::transformToWorldFrame(contact_tang_unit_body, current.pose.orientation);
next.twist.linear += contact_tang_unit * friction_mag;
Expand All @@ -188,9 +202,9 @@ class FastPhysicsEngine : public PhysicsEngineBase {
next.twist.angular *= 0.9f;

//there is no acceleration during collision response
next.accelerations.linear = Vector3r::Zero();
next.accelerations.angular = Vector3r::Zero();
//next.accelerations.linear = Vector3r::Zero();
//next.accelerations.angular = Vector3r::Zero();

next.pose = current.pose;
if (enable_ground_lock && ground_collision) {
float pitch, roll, yaw;
Expand All @@ -205,6 +219,11 @@ class FastPhysicsEngine : public PhysicsEngineBase {
next.twist.linear = Vector3r::Zero();
next.pose.position = collision_info.position;
grounded = true;

// but we do want to "feel" the ground when we hit it (we should see a small z-acc bump)
// equal and opposite our downward velocity.
next.accelerations.linear = -0.5 * body.getMass() * vcur_avg;

//throttledLogOutput("*** Triggering ground lock", 0.1);
}
else
Expand All @@ -230,33 +249,6 @@ class FastPhysicsEngine : public PhysicsEngineBase {
}
}

//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 ************************/
// real_T min_z_over_ground = body.getEnvironment().getState().min_z_over_ground;
// grounded_ = 0;
// if (min_z_over_ground <= next.pose.position.z()) {
// grounded_ = 1;
// next.pose.position.z() = min_z_over_ground;

// real_T z_proj = static_cast<real_T>(next.twist.linear.z() + next.accelerations.linear.z() * dt);
// if (Utils::isDefinitelyLessThan(0.0f, z_proj)) {
// grounded_ = 2;
// next.twist = Twist::zero();
// next.accelerations.linear = Vector3r::Zero();
// next.accelerations.angular = Vector3r::Zero();
// //reset roll/pitch - px4 seems to have issue with this
// real_T r, p, y;
// VectorMath::toEulerianAngle(current.pose.orientation, p, r, y);
// next.pose.orientation = VectorMath::toQuaternion(0, 0, y);

// next_wrench = Wrench::zero();
// }
// }

// return grounded_ != 0;
//}

static Wrench getDragWrench(const PhysicsBody& body, const Quaternionr& orientation,
const Vector3r& linear_vel, const Vector3r& angular_vel_body)
{
Expand Down Expand Up @@ -327,9 +319,10 @@ class FastPhysicsEngine : public PhysicsEngineBase {
const Wrench body_wrench = getBodyWrench(body, current.pose.orientation);

if (grounded) {
// make it stick to the ground until we see significant body wrench forces.
// make it stick to the ground until we see body wrench force greater than gravity.
float normalizedForce = body_wrench.force.squaredNorm();
if (normalizedForce > kSurfaceTension)
float normalizedGravity = body.getEnvironment().getState().gravity.squaredNorm();
if (normalizedForce >= normalizedGravity)
{
//throttledLogOutput("*** Losing ground lock due to body_wrench " + VectorMath::toString(body_wrench.force), 0.1);
grounded = false;
Expand Down Expand Up @@ -446,7 +439,6 @@ 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_;
bool grounded_ = false;
Expand Down
1 change: 0 additions & 1 deletion AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,7 +86,6 @@ class MagnetometerSimple : public MagnetometerBase {
if (params_.dynamic_reference_source)
updateReference(ground_truth);

// Calculate the magnetic field noise.
// Calculate the magnetic field noise.
output.magnetic_field_body = VectorMath::transformToBodyFrame(magnetic_field_true_,
ground_truth.kinematics->pose.orientation, true) * params_.scale_factor
Expand Down
2 changes: 1 addition & 1 deletion AirLib/include/vehicles/multirotor/MultiRotorParams.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ class MultiRotorParams {
//angular coefficient is usually 10X smaller than linear, however we should replace this with exact number
//http://physics.stackexchange.com/q/304742/14061
real_T angular_drag_coefficient = linear_drag_coefficient;
real_T restitution = 0.15f;
real_T restitution = 0.15f; // value of 1 would result in perfectly elastic collisions, 0 would be completely inelastic.
real_T friction = 0.7f;
EnabledSensors enabled_sensors;
RotorParams rotor_params;
Expand Down
1 change: 0 additions & 1 deletion PythonClient/AirSimClient.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -217,7 +217,6 @@ def reset(self):
self.client.call('reset')

def confirmConnection(self):
print('Waiting for connection: ', end='')
home = self.getHomeGeoPoint()
while ((home.latitude == 0 and home.longitude == 0 and home.altitude == 0) or
math.isnan(home.latitude) or math.isnan(home.longitude) or math.isnan(home.altitude)):
Expand Down
3 changes: 2 additions & 1 deletion PythonClient/path.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,9 @@

# this method is async and we are not waiting for the result since we are passing max_wait_seconds=0.
result = client.moveOnPath([Vector3r(0,-253,z),Vector3r(125,-253,z),Vector3r(125,0,z),Vector3r(0,0,z),Vector3r(0,0,-20)],
15, 65,
12, 120,
DrivetrainType.ForwardOnly, YawMode(False,0), 20, 1)
client.moveToPosition(0,0,z,1)
client.land()
client.armDisarm(False)
client.enableApiControl(False)
8 changes: 4 additions & 4 deletions PythonClient/survey.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,10 +80,10 @@ def start(self):
args = sys.argv
args.pop(0)
arg_parser = argparse.ArgumentParser("Usage: survey boxsize stripewidth altitude")
arg_parser.add_argument("--size", type=float, help="size of the box to survey")
arg_parser.add_argument("--stripewidth", type=float, help="stripe width of survey (in meters)")
arg_parser.add_argument("--altitude", type=float, help="altitude of survey (in positive meters)")
arg_parser.add_argument("--speed", type=float, help="speed of survey (in meters/second)")
arg_parser.add_argument("--size", type=float, help="size of the box to survey", default=50)
arg_parser.add_argument("--stripewidth", type=float, help="stripe width of survey (in meters)", default=10)
arg_parser.add_argument("--altitude", type=float, help="altitude of survey (in positive meters)", default=30)
arg_parser.add_argument("--speed", type=float, help="speed of survey (in meters/second)", default=5)
args = arg_parser.parse_args(args)
nav = SurveyNavigator(args)
nav.start()

0 comments on commit 55e9ecb

Please sign in to comment.