Update other projects + Unity for kinematics/environment fix

AirLib/include/physics/DebugPhysicsBody.hpp

@@ -42,7 +42,6 @@ class DebugPhysicsBody : public PhysicsBody {
     {
         PhysicsBody::updateKinematics(kinematics);
 
-        const auto& kinematics = this->getKinematics();
         std::cout << " Pos: " << VectorMath::toString(kinematics.pose.position);
         std::cout << " Ori: " << VectorMath::toString(kinematics.pose.orientation) << std::endl;
         std::cout << " Lin Vel: " << VectorMath::toString(kinematics.twist.linear);

AirLibUnitTests/SimpleFlightTest.hpp

@@ -28,7 +28,18 @@ class SimpleFlightTest : public TestBase
             std::make_shared<SensorFactory>());
         auto api = params->createMultirotorApi();
 
-        MultiRotor vehicle(params.get(), api.get(), Pose(), GeoPoint());
+        std::unique_ptr<msr::airlib::Kinematics> kinematics;
+        std::unique_ptr<msr::airlib::Environment> environment;
+        Kinematics::State initial_kinematic_state = Kinematics::State::zero();;
+        initial_kinematic_state.pose = Pose();
+        kinematics.reset(new Kinematics(initial_kinematic_state));
+
+        Environment::State initial_environment;
+        initial_environment.position = initial_kinematic_state.pose.position;
+        initial_environment.geo_point = GeoPoint();
+        environment.reset(new Environment(initial_environment));
+
+        MultiRotor vehicle(params.get(), api.get(), kinematics.get(), environment.get());
 
         std::vector<UpdatableObject*> vehicles = { &vehicle };
         std::unique_ptr<PhysicsEngineBase> physics_engine(new FastPhysicsEngine());

Examples/StandAlonePhysics.hpp

@@ -22,9 +22,10 @@ class StandAlonePhysics {
         msr::airlib::Environment::State initial_environment;
         initial_environment.position = initial_kinematics.pose.position;
         Environment environment(initial_environment);
+        Kinematics kinematics(initial_kinematics);
 
         DebugPhysicsBody body;
-        body.initialize(initial_kinematics, &environment);
+        body.initialize(&kinematics, &environment);
         body.reset();
 
         //create physics engine

Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp

@@ -8,15 +8,22 @@
 PawnSimApi::PawnSimApi(const Params& params)
 	: params_(params), ned_transform_(*params.global_transform)
 {
-	image_capture_.reset(new UnityImageCapture(params.vehicle_name));
+}
+
+void PawnSimApi::initialize()
+{
+	image_capture_.reset(new UnityImageCapture(params_.vehicle_name));
 
-	msr::airlib::Environment::State initial_environment;
-	initial_environment.position = getPose().position;
+    Kinematics::State initial_kinematic_state = Kinematics::State::zero();;
+    initial_kinematic_state.pose = getPose();
+    kinematics_.reset(new Kinematics(initial_kinematic_state));
+    Environment::State initial_environment;
+    initial_environment.position = initial_kinematic_state.pose.position;
 	initial_environment.geo_point = params_.home_geopoint;
-	environment_.reset(new msr::airlib::Environment(initial_environment));
+	environment_.reset(new Environment(initial_environment));
 
 	//initialize state
-	UnityTransform initialTransform = GetTransformFromUnity(params.vehicle_name.c_str());
+	UnityTransform initialTransform = GetTransformFromUnity(params_.vehicle_name.c_str());
 	initial_state_.mesh_origin = initialTransform.Position;
 	setStartPosition(initialTransform.Position, initialTransform.Rotation);
 
@@ -43,6 +50,9 @@ void PawnSimApi::setStartPosition(const AirSimVector& position, const AirSimQuat
 
 void PawnSimApi::pawnTick(float dt)
 {
+    //default behavior is to call update every tick
+    //for custom physics engine, this method should be overridden and update should be
+    //called from every physics tick
 	update();
 	updateRenderedState(dt);
 	updateRendering(dt);
@@ -141,19 +151,29 @@ void PawnSimApi::reset()
 	VehicleSimApiBase::reset();
 	state_ = initial_state_;
 	rc_data_ = msr::airlib::RCData();
+    kinematics_->reset();
 	environment_->reset();
 }
 
 void PawnSimApi::update()
 {
-	//update position from kinematics so we have latest position after physics update
-	environment_->setPosition(kinematics_.pose.position);
+    //sync environment from kinematics
+	environment_->setPosition(kinematics_->getPose().position);
 	environment_->update();
-	//kinematics_->update();
 
 	VehicleSimApiBase::update();
 }
 
+void PawnSimApi::reportState(msr::airlib::StateReporter& reporter)
+{
+    msr::airlib::VehicleSimApiBase::reportState(reporter);
+    kinematics_->reportState(reporter);
+    environment_->reportState(reporter);
+    // report actual location in unreal coordinates so we can plug that into the UE editor to move the drone.
+    //FVector unrealPosition = getUUPosition();
+    //reporter.writeValue("unreal pos", Vector3r(unrealPosition.X, unrealPosition.Y, unrealPosition.Z));
+}
+
 PawnSimApi::CollisionInfo PawnSimApi::getCollisionInfo() const
 {
 	return state_.collision_info;
@@ -218,13 +238,10 @@ bool PawnSimApi::canTeleportWhileMove()  const
 	return !state_.collisions_enabled || (state_.collision_info.has_collided && !state_.was_last_move_teleport && state_.passthrough_enabled);
 }
 
-const msr::airlib::Kinematics::State* PawnSimApi::getPawnKinematics() const
-{
-	return &kinematics_;
-}
-
 void PawnSimApi::updateRenderedState(float dt)
 {
+    //by default we update kinematics from UE pawn
+    //if SimMod uses its own physics engine then this should be overriden
 	updateKinematics(dt);
 }
 
@@ -236,14 +253,23 @@ void PawnSimApi::updateRendering(float dt)
 
 const msr::airlib::Kinematics::State* PawnSimApi::getGroundTruthKinematics() const
 {
-	return &kinematics_;
+	return &kinematics_->getState();
 }
 
 const msr::airlib::Environment* PawnSimApi::getGroundTruthEnvironment() const
 {
 	return environment_.get();
 }
 
+msr::airlib::Kinematics* PawnSimApi::getKinematics()
+{
+    return kinematics_.get();
+}
+msr::airlib::Environment* PawnSimApi::getEnvironment()
+{
+    return environment_.get();
+}
+
 std::string PawnSimApi::getRecordFileLine(bool is_header_line) const
 {
 	if (is_header_line) 
@@ -273,14 +299,15 @@ std::string PawnSimApi::getRecordFileLine(bool is_header_line) const
 
 void PawnSimApi::updateKinematics(float dt)
 {
-	const auto last_kinematics = kinematics_;
-
-	kinematics_.pose = getPose();
-
-	kinematics_.twist.linear = UnityUtilities::Convert_to_Vector3r(GetVelocity(getVehicleName().c_str()));
-	kinematics_.twist.angular = msr::airlib::VectorMath::toAngularVelocity(
-		kinematics_.pose.orientation, last_kinematics.pose.orientation, dt);
-
-	kinematics_.accelerations.linear = (kinematics_.twist.linear - last_kinematics.twist.linear) / dt;
-	kinematics_.accelerations.angular = (kinematics_.twist.angular - last_kinematics.twist.angular) / dt;
+    //update kinematics from pawn's movement instead of physics engine
+    auto next_kinematics = kinematics_->getState();
+    next_kinematics.pose = getPose();
+    next_kinematics.twist.linear = UnityUtilities::Convert_to_Vector3r(GetVelocity(getVehicleName().c_str()));
+    next_kinematics.twist.angular = msr::airlib::VectorMath::toAngularVelocity(
+        kinematics_->getPose().orientation, next_kinematics.pose.orientation, dt);
+    //TODO: update other fields?
+    next_kinematics.accelerations.linear = (next_kinematics.twist.linear - kinematics_->getTwist().linear) / dt;
+    next_kinematics.accelerations.angular = (next_kinematics.twist.angular - kinematics_->getTwist().angular) / dt;
+    kinematics_->setState(next_kinematics);
+    kinematics_->update();
 }

Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h

@@ -15,6 +15,8 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase
 {
 private:
 	typedef msr::airlib::AirSimSettings AirSimSettings;
+    typedef msr::airlib::Kinematics Kinematics;
+    typedef msr::airlib::Environment Environment;
 
 public:
 	typedef msr::airlib::GeoPoint GeoPoint;
@@ -55,10 +57,12 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase
 	void updateKinematics(float dt);
 
 protected:
-	const msr::airlib::Kinematics::State* getPawnKinematics() const;
 	AirSimPose GetInitialPose();
+    msr::airlib::Kinematics* getKinematics();
+    msr::airlib::Environment* getEnvironment();
 
 public:
+    virtual void initialize() override;
 	PawnSimApi(const Params& params);
 	virtual void reset() override;
 	virtual void update() override;
@@ -82,6 +86,7 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase
 	virtual const msr::airlib::Kinematics::State* getGroundTruthKinematics() const override;
 	virtual const msr::airlib::Environment* getGroundTruthEnvironment() const override;
 	virtual std::string getRecordFileLine(bool is_header_line) const override;
+    virtual void reportState(msr::airlib::StateReporter& reporter) override;
 	void OnCollision(msr::airlib::CollisionInfo collisionInfo);
 	const NedTransform& getNedTransform() const;
 	virtual void pawnTick(float dt);
@@ -107,6 +112,6 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase
 	};
 	State state_, initial_state_;
 
-	msr::airlib::Kinematics::State kinematics_;
-	std::unique_ptr<msr::airlib::Environment> environment_;
+    std::unique_ptr<msr::airlib::Kinematics> kinematics_;
+    std::unique_ptr<msr::airlib::Environment> environment_;
 };

Unity/AirLibWrapper/AirsimWrapper/Source/SimMode/SimModeBase.cpp

@@ -233,7 +233,9 @@ std::unique_ptr<PawnSimApi> SimModeBase::createVehicleSimApi(
 	const PawnSimApi::Params& pawn_sim_api_params) const
 {
 	unused(pawn_sim_api_params);
-	return std::unique_ptr<PawnSimApi>();
+    auto sim_api = std::unique_ptr<PawnSimApi>();
+    sim_api->initialize();
+    return sim_api;
 }
 
 msr::airlib::VehicleApiBase* SimModeBase::getVehicleApi(const PawnSimApi::Params& pawn_sim_api_params,

Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.cpp

@@ -9,18 +9,28 @@
 
 CarPawnSimApi::CarPawnSimApi(const Params& params,
 	const CarPawnApi::CarControls&  keyboard_controls, std::string car_name)
-	: PawnSimApi(params),
+	: PawnSimApi(params), params_(params),
 	keyboard_controls_(keyboard_controls), car_name_(car_name)
 {
 	createVehicleApi(static_cast<CarPawn*>(params.pawn), params.home_geopoint);
 	joystick_controls_ = CarPawnApi::CarControls();
 }
 
+void CarPawnSimApi::initialize()
+{
+    PawnSimApi::initialize();
+
+    createVehicleApi(static_cast<CarPawn*>(params_.pawn), params_.home_geopoint);
+
+    //TODO: should do reset() here?
+    joystick_controls_ = CarPawnApi::CarControls();
+}
+
 void CarPawnSimApi::createVehicleApi(CarPawn* pawn, const msr::airlib::GeoPoint& home_geopoint)
 {
 	std::shared_ptr<UnitySensorFactory> sensor_factory = std::make_shared<UnitySensorFactory>(car_name_, &getNedTransform());
 
-	vehicle_api_ = std::unique_ptr<msr::airlib::CarApiBase>(new CarPawnApi(pawn, getPawnKinematics(), home_geopoint,
+	vehicle_api_ = std::unique_ptr<msr::airlib::CarApiBase>(new CarPawnApi(pawn, getGroundTruthKinematics(), home_geopoint,
 		getVehicleSetting(), sensor_factory, car_name_,
 		(*getGroundTruthKinematics()), (*getGroundTruthEnvironment())));
 }
@@ -169,10 +179,10 @@ void CarPawnSimApi::update()
 	PawnSimApi::update();
 }
 
-void CarPawnSimApi::reportState(StateReporter& reporter)
-{
-	// report actual location in unreal coordinates so we can plug that into the UE editor to move the drone.
-	AirSimPose pose = GetPose(getVehicleName().c_str());
-	reporter.writeValue("unreal pos", Vector3r(pose.position.x, pose.position.y, pose.position.z));
-}
+//void CarPawnSimApi::reportState(StateReporter& reporter)
+//{
+//	// report actual location in unreal coordinates so we can plug that into the UE editor to move the drone.
+//	AirSimPose pose = GetPose(getVehicleName().c_str());
+//	reporter.writeValue("unreal pos", Vector3r(pose.position.x, pose.position.y, pose.position.z));
+//}
 //*** End: UpdatableState implementation ***//

Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.h

@@ -18,11 +18,12 @@ class CarPawnSimApi : public PawnSimApi
 	void updateCarControls();
 
 public:
+    virtual void initialize() override;
 	CarPawnSimApi(const Params& params, const CarPawnApi::CarControls&  keyboard_controls, std::string car_name);
 	virtual ~CarPawnSimApi() = default;
 	virtual void reset() override;
 	virtual void update() override;
-	virtual void reportState(StateReporter& reporter) override;
+	//virtual void reportState(StateReporter& reporter) override;
 	virtual std::string getRecordFileLine(bool is_header_line) const override;
 	virtual void updateRenderedState(float dt) override;
 	virtual void updateRendering(float dt) override;
@@ -33,6 +34,8 @@ class CarPawnSimApi : public PawnSimApi
 	}
 
 private:
+    Params params_;
+
 	std::unique_ptr<msr::airlib::CarApiBase> vehicle_api_;
 	std::vector<std::string> vehicle_api_messages_;
 	CarPawnApi::CarControls joystick_controls_;

Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/SimModeCar.cpp

@@ -91,7 +91,7 @@ std::unique_ptr<PawnSimApi> SimModeCar::createVehicleSimApi(const PawnSimApi::Pa
 
 	auto vehicle_sim_api = std::unique_ptr<PawnSimApi>(new CarPawnSimApi(pawn_sim_api_params,
 		vehicle_pawn->getKeyBoardControls(), vehicle_name_));
-
+    vehicle_sim_api->initialize();
 	vehicle_sim_api->reset();
 	return vehicle_sim_api;
 }

Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp

@@ -14,34 +14,29 @@ MultirotorPawnSimApi::MultirotorPawnSimApi(const Params& params) : PawnSimApi(pa
 	VectorMath::toEulerianAngle(pose.orientation, pitch, roll, yaw);
 	pose.orientation = VectorMath::toQuaternion(0, 0, yaw);
 	setPose(pose, false);
-
-	//create vehicle API
-	auto sensor_factory = std::make_shared<UnitySensorFactory>(getVehicleName(), &getNedTransform());
-	vehicle_params_ = MultiRotorParamsFactory::createConfig(getVehicleSetting(), sensor_factory);
-	vehicle_api_ = vehicle_params_->createMultirotorApi();
-	//setup physics vehicle
-	phys_vehicle_ = std::unique_ptr<MultiRotor>(new MultiRotor(vehicle_params_.get(), vehicle_api_.get(),
-		getPose(), params.home_geopoint));
-	rotor_count_ = phys_vehicle_->wrenchVertexCount();
-	rotor_info_.assign(rotor_count_, RotorInfo());
-
-	vehicle_api_->setSimulatedGroundTruth(getGroundTruthKinematics(), getGroundTruthEnvironment());
-
-	//initialize private vars
-	last_phys_pose_ = pending_phys_pose_ = Pose::nanPose();
-	pending_pose_status_ = PendingPoseStatus::NonePending;
-	reset_pending_ = false;
-	did_reset_ = false;
-}
-
-const msr::airlib::Kinematics::State* MultirotorPawnSimApi::getGroundTruthKinematics() const
-{
-	return &phys_vehicle_->getKinematics();
 }
 
-const msr::airlib::Environment* MultirotorPawnSimApi::getGroundTruthEnvironment() const
+void MultirotorPawnSimApi::initialize()
 {
-	return &phys_vehicle_->getEnvironment();
+    PawnSimApi::initialize();
+
+    //create vehicle API
+    std::shared_ptr<UnitySensorFactory> sensor_factory = std::make_shared<UnitySensorFactory>(getVehicleName(), &getNedTransform());
+    vehicle_params_ = MultiRotorParamsFactory::createConfig(getVehicleSetting(), sensor_factory);
+    vehicle_api_ = vehicle_params_->createMultirotorApi();
+    //setup physics vehicle
+    phys_vehicle_ = std::unique_ptr<MultiRotor>(new MultiRotor(vehicle_params_.get(), vehicle_api_.get(),
+        getKinematics(), getEnvironment()));
+    rotor_count_ = phys_vehicle_->wrenchVertexCount();
+    rotor_info_.assign(rotor_count_, RotorInfo());
+
+    vehicle_api_->setSimulatedGroundTruth(getGroundTruthKinematics(), getGroundTruthEnvironment());
+
+    //initialize private vars
+    last_phys_pose_ = pending_phys_pose_ = Pose::nanPose();
+    pending_pose_status_ = PendingPoseStatus::NonePending;
+    reset_pending_ = false;
+    did_reset_ = false;
 }
 
 void MultirotorPawnSimApi::updateRenderedState(float dt)
@@ -166,9 +161,9 @@ void MultirotorPawnSimApi::update()
 
 void MultirotorPawnSimApi::reportState(StateReporter& reporter)
 {
-	// report actual location in unreal coordinates so we can plug that into the UE editor to move the drone.
-	AirSimPose pose = GetPose(getVehicleName().c_str());
-	reporter.writeValue("unreal pos", Vector3r(pose.position.x, pose.position.y, pose.position.z));
+	//// report actual location in unreal coordinates so we can plug that into the UE editor to move the drone.
+	//AirSimPose pose = GetPose(getVehicleName().c_str());
+	//reporter.writeValue("unreal pos", Vector3r(pose.position.x, pose.position.y, pose.position.z));
 	phys_vehicle_->reportState(reporter);
 }
 

Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h

@@ -19,6 +19,8 @@ class MultirotorPawnSimApi : public PawnSimApi
 	typedef MultirotorPawnEvents::RotorInfo RotorInfo;
 
 public:
+    virtual void initialize() override;
+
 	MultirotorPawnSimApi(const Params& params);
 	virtual ~MultirotorPawnSimApi() = default;
 	virtual void updateRenderedState(float dt) override;
@@ -28,8 +30,6 @@ class MultirotorPawnSimApi : public PawnSimApi
 	virtual void reportState(StateReporter& reporter) override;
 	virtual UpdatableObject* getPhysicsBody() override;
 	virtual void setPose(const Pose& pose, bool ignore_collision) override;
-	virtual const msr::airlib::Kinematics::State* getGroundTruthKinematics() const override;
-	virtual const msr::airlib::Environment* getGroundTruthEnvironment() const override;
 
 	msr::airlib::MultirotorApiBase* getVehicleApi() const
 	{