Merge branch 'master' into PR/simLoadLevel_simGetSetObjectPose

AirLib/include/api/RpcLibClientBase.hpp

@@ -101,7 +101,7 @@ class RpcLibClientBase {
     CollisionInfo simGetCollisionInfo(const std::string& vehicle_name = "") const;
 
     CameraInfo simGetCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "") const;
-    void simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation, const std::string& vehicle_name = "");
+    void simSetCameraPose(const std::string& camera_name, const Pose& pose, const std::string& vehicle_name = "");
     void simSetCameraFov(const std::string& camera_name, float fov_degrees, const std::string& vehicle_name = "");
 
     msr::airlib::Kinematics::State simGetGroundTruthKinematics(const std::string& vehicle_name = "") const;

AirLib/include/api/VehicleSimApiBase.hpp

@@ -54,7 +54,7 @@ class VehicleSimApiBase : public msr::airlib::UpdatableObject {
     virtual const msr::airlib::Environment* getGroundTruthEnvironment() const = 0;
 
     virtual CameraInfo getCameraInfo(const std::string& camera_name) const = 0;
-    virtual void setCameraOrientation(const std::string& camera_name, const Quaternionr& orientation) = 0;
+    virtual void setCameraPose(const std::string& camera_name, const Pose& pose) = 0;
     virtual void setCameraFoV(const std::string& camera_name, float fov_degrees) = 0;
 
     virtual CollisionInfo getCollisionInfo() const = 0;

AirLib/include/common/VectorMath.hpp

@@ -324,6 +324,9 @@ class VectorMathT {
 
 	static Vector3T toAngularVelocity(const QuaternionT& start, const QuaternionT& end, RealT dt)
 	{
+		if (dt == 0)
+			return Vector3T(0, 0, 0);
+
 		RealT p_s, r_s, y_s;
 		toEulerianAngle(start, p_s, r_s, y_s);
 

AirLib/include/vehicles/car/api/CarApiBase.hpp

@@ -68,6 +68,16 @@ class CarApiBase : public VehicleApiBase  {
               kinematics_estimated(kinematics_estimated_val), timestamp(timestamp_val)
         {
         }
+
+        //shortcuts
+        const Vector3r& getPosition() const
+        {
+            return kinematics_estimated.pose.position;
+        }
+        const Quaternionr& getOrientation() const
+        {
+            return kinematics_estimated.pose.orientation;
+        }
     };
 
 public:
@@ -151,4 +161,4 @@ class CarApiBase : public VehicleApiBase  {
 
 
 }} //namespace
-#endif
+#endif

AirLib/include/vehicles/multirotor/MultiRotorParams.hpp

@@ -36,12 +36,12 @@ class MultiRotorParams {
         Vector3r body_box;
 
         /*********** optional parameters with defaults ***********/
-        real_T linear_drag_coefficient = 1.3f / 4.0f; 
+        real_T linear_drag_coefficient = 1.3f / 4.0f;
         //sample value 1.3 from http://klsin.bpmsg.com/how-fast-can-a-quadcopter-fly/, but divided by 4 to account
         // for nice streamlined frame design and allow higher top speed which is more fun.
         //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 angular_drag_coefficient = linear_drag_coefficient;
         real_T restitution = 0.55f; // value of 1 would result in perfectly elastic collisions, 0 would be completely inelastic.
         real_T friction = 0.5f;
         RotorParams rotor_params;
@@ -100,37 +100,37 @@ class MultiRotorParams {
     /// which shows that given an array of 4 motors, the first is placed top right and flies counter clockwise (CCW) and
     /// the second is placed bottom left, and also flies CCW.  The third in the array is placed top left and flies clockwise (CW)
     /// while the last is placed bottom right and also flies clockwise.  This is how the 4 items in the arm_lengths and
-    /// arm_angles arrays will be used.  So arm_lengths is 4 numbers (in meters) where four arm lengths, 0 is top right, 
-    /// 1 is bottom left, 2 is top left and 3 is bottom right.  arm_angles is 4 numbers (in degrees)  relative to forward vector (0,1), 
-    /// provided in the same order where 0 is top right, 1 is bottom left, 2 is top left and 3 is bottom right, so for example, 
+    /// arm_angles arrays will be used.  So arm_lengths is 4 numbers (in meters) where four arm lengths, 0 is top right,
+    /// 1 is bottom left, 2 is top left and 3 is bottom right.  arm_angles is 4 numbers (in degrees)  relative to forward vector (0,1),
+    /// provided in the same order where 0 is top right, 1 is bottom left, 2 is top left and 3 is bottom right, so for example,
     /// the angles for a regular symmetric X pattern would be 45, 225, 315, 135.  The rotor_z is the offset of each motor upwards
-    /// relative to the center of mass (in meters). 
+    /// relative to the center of mass (in meters).
     static void initializeRotorQuadX(vector<RotorPose>& rotor_poses /* the result we are building */,
-        uint rotor_count /* must be 4 */, 
-        real_T arm_lengths[], 
+        uint rotor_count /* must be 4 */,
+        real_T arm_lengths[],
         real_T rotor_z /* z relative to center of gravity */)
     {
         Vector3r unit_z(0, 0, -1);  //NED frame
         if (rotor_count == 4) {
             rotor_poses.clear();
 
-            /* Note: rotor_poses are built in this order:              
+            /* Note: rotor_poses are built in this order:
                  x-axis
             (2)  |   (0)
                  |
             -------------- y-axis
                  |
-            (1)  |   (3)		
+            (1)  |   (3)
             */
             // vectors below are rotated according to NED left hand rule (so the vectors are rotated counter clockwise).
             Quaternionr quadx_rot(AngleAxisr(M_PIf / 4, unit_z));
             rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(0, arm_lengths[0], rotor_z), quadx_rot, true),
                 unit_z, RotorTurningDirection::RotorTurningDirectionCCW);
             rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(0, -arm_lengths[1], rotor_z), quadx_rot, true),
                 unit_z, RotorTurningDirection::RotorTurningDirectionCCW);
-            rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(arm_lengths[2], 0, rotor_z), quadx_rot, true), 
+            rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(arm_lengths[2], 0, rotor_z), quadx_rot, true),
                 unit_z, RotorTurningDirection::RotorTurningDirectionCW);
-            rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(-arm_lengths[3], 0, rotor_z), quadx_rot, true), 
+            rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(-arm_lengths[3], 0, rotor_z), quadx_rot, true),
                 unit_z, RotorTurningDirection::RotorTurningDirectionCW);
         }
         else
@@ -153,8 +153,8 @@ class MultiRotorParams {
                    \  /
                     \/
                (1)-------(0) y-axis
-                    /\                
-                   /  \ 
+                    /\
+                   /  \
                  (5)  (3)
 
             */
@@ -200,9 +200,9 @@ class MultiRotorParams {
         inertia = Matrix3x3r::Zero();
 
         //http://farside.ph.utexas.edu/teaching/336k/Newtonhtml/node64.html
-        inertia(0, 0) = box_mass / 12.0f * (body_box.y()*body_box.y() + body_box.z()*body_box.z()); 
-        inertia(1, 1) = box_mass / 12.0f * (body_box.x()*body_box.x() + body_box.z()*body_box.z()); 
-        inertia(2, 2) = box_mass / 12.0f * (body_box.x()*body_box.x() + body_box.y()*body_box.y()); 
+        inertia(0, 0) = box_mass / 12.0f * (body_box.y()*body_box.y() + body_box.z()*body_box.z());
+        inertia(1, 1) = box_mass / 12.0f * (body_box.x()*body_box.x() + body_box.z()*body_box.z());
+        inertia(2, 2) = box_mass / 12.0f * (body_box.x()*body_box.x() + body_box.y()*body_box.y());
 
         for (size_t i = 0; i < rotor_poses.size(); ++i) {
             const auto& pos = rotor_poses.at(i).position;
@@ -212,6 +212,194 @@ class MultiRotorParams {
         }
     }
 
+    // Some Frame types which can be used by different firmwares
+    // Specific frame configurations, modifications can be done in the Firmware Params
+
+    void setupFrameGenericQuad(Params& params)
+    {
+        //set up arm lengths
+        //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution
+        params.rotor_count = 4;
+        std::vector<real_T> arm_lengths(params.rotor_count, 0.2275f);
+
+        //set up mass
+        params.mass = 1.0f; //can be varied from 0.800 to 1.600
+        real_T motor_assembly_weight = 0.055f;  //weight for MT2212 motor for F450 frame
+        real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight;
+
+        // using rotor_param default, but if you want to change any of the rotor_params, call calculateMaxThrust() to recompute the max_thrust
+        // given new thrust coefficients, motor max_rpm and propeller diameter.
+        params.rotor_params.calculateMaxThrust();
+
+        //set up dimensions of core body box or abdomen (not including arms).
+        params.body_box.x() = 0.180f; params.body_box.y() = 0.11f; params.body_box.z() = 0.040f;
+        real_T rotor_z = 2.5f / 100;
+
+        //computer rotor poses
+        initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z);
+
+        //compute inertia matrix
+        computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight);
+    }
+
+    void setupFrameGenericHex(Params& params)
+    {
+        //set up arm lengths
+        //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution
+        params.rotor_count = 6;
+        std::vector<real_T> arm_lengths(params.rotor_count, 0.2275f);
+
+        //set up mass
+        params.mass = 1.0f; //can be varied from 0.800 to 1.600
+        real_T motor_assembly_weight = 0.055f;  //weight for MT2212 motor for F450 frame
+        real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight;
+
+        // using rotor_param default, but if you want to change any of the rotor_params, call calculateMaxThrust() to recompute the max_thrust
+        // given new thrust coefficients, motor max_rpm and propeller diameter.
+        params.rotor_params.calculateMaxThrust();
+
+        //set up dimensions of core body box or abdomen (not including arms).
+        params.body_box.x() = 0.180f; params.body_box.y() = 0.11f; params.body_box.z() = 0.040f;
+        real_T rotor_z = 2.5f / 100;
+
+        //computer rotor poses
+        initializeRotorHexX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z);
+
+        //compute inertia matrix
+        computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight);
+    }
+
+    void setupFrameFlamewheel(Params& params)
+    {
+        //set up arm lengths
+        //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution
+        params.rotor_count = 4;
+        std::vector<real_T> arm_lengths(params.rotor_count, 0.225f);
+
+        //set up mass
+        params.mass = 1.635f;
+        real_T motor_assembly_weight = 0.052f;
+        real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight;
+
+        params.rotor_params.C_T = 0.11f;
+        params.rotor_params.C_P = 0.047f;
+        params.rotor_params.max_rpm = 9500;
+        params.rotor_params.calculateMaxThrust();
+        params.linear_drag_coefficient *= 4; // make top speed more real.
+
+        //set up dimensions of core body box or abdomen (not including arms).
+        params.body_box.x() = 0.16f; params.body_box.y() = 0.10f; params.body_box.z() = 0.14f;
+        real_T rotor_z = 0.15f;
+
+        //computer rotor poses
+        initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z);
+
+        //compute inertia matrix
+        computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight);
+    }
+
+    void setupFrameFlamewheelFLA(Params& params)
+    {
+        //set up arm lengths
+        //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution
+        params.rotor_count = 4;
+        std::vector<real_T> arm_lengths(params.rotor_count, 0.225f);
+
+        //set up mass
+        params.mass = 2.25f;
+        real_T motor_assembly_weight = 0.1f;
+        real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight;
+
+        params.rotor_params.C_T = 0.2f;
+        params.rotor_params.C_P = 0.1f;
+        params.rotor_params.max_rpm = 9324;
+        params.rotor_params.calculateMaxThrust();
+        params.linear_drag_coefficient *= 4; // make top speed more real.
+
+        //set up dimensions of core body box or abdomen (not including arms).
+        params.body_box.x() = 0.16f; params.body_box.y() = 0.10f; params.body_box.z() = 0.14f;
+        real_T rotor_z = 0.15f;
+
+        //computer rotor poses
+        initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z);
+
+        //compute inertia matrix
+        computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight);
+    }
+
+    void setupFrameBlacksheep(Params& params)
+    {
+        /*
+        Motor placement:
+        x
+        (2)  |   (0)
+        |
+        ------------ y
+        |
+        (1)  |   (3)
+        |
+
+        */
+        //set up arm lengths
+        //dimensions are for Team Blacksheep Discovery (http://team-blacksheep.com/products/product:98)
+        params.rotor_count = 4;
+        std::vector<real_T> arm_lengths;
+
+        Vector3r unit_z(0, 0, -1);  //NED frame
+
+        // relative to Forward vector in the order (0,3,1,2) required by quad X pattern
+        // http://ardupilot.org/copter/_images/MOTORS_QuadX_QuadPlus.jpg
+        arm_lengths.push_back(0.22f);
+        arm_lengths.push_back(0.255f);
+        arm_lengths.push_back(0.22f);
+        arm_lengths.push_back(0.255f);
+
+        // note: the Forward vector is actually the "x" axis, and the AngleAxisr rotation is pointing down and is left handed, so this means the rotation
+        // is counter clockwise, so the vector (arm_lengths[i], 0) is the X-axis, so the CCW rotations to position each arm correctly are listed below:
+        // See measurements here: http://diydrones.com/profiles/blogs/arducopter-tbs-discovery-style (angles reversed because we are doing CCW rotation)
+        std::vector<real_T> arm_angles;
+        arm_angles.push_back(-55.0f);
+        arm_angles.push_back(125.0f);
+        arm_angles.push_back(55.0f);
+        arm_angles.push_back(-125.0f);
+
+        // quad X pattern
+        std::vector<RotorTurningDirection> rotor_directions;
+        rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCCW);
+        rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCCW);
+        rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCW);
+        rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCW);
+
+        // data from
+        // http://dronesvision.net/team-blacksheep-750kv-motor-esc-set-for-tbs-discovery-fpv-quadcopter/
+        //set up mass
+        params.mass = 2.0f; //can be varied from 0.800 to 1.600
+        real_T motor_assembly_weight = 0.052f;  // weight for TBS motors
+        real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight;
+
+        // the props we are using a E-Prop, which I didn't find in UIUC database, but this one is close:
+        // http://m-selig.ae.illinois.edu/props/volume-2/plots/ef_130x70_static_ctcp.png
+        params.rotor_params.C_T = 0.11f;
+        params.rotor_params.C_P = 0.047f;
+        params.rotor_params.max_rpm = 9500;
+        params.rotor_params.calculateMaxThrust();
+
+        //set up dimensions of core body box or abdomen (not including arms).
+        params.body_box.x() = 0.20f; params.body_box.y() = 0.12f; params.body_box.z() = 0.04f;
+        real_T rotor_z = 2.5f / 100;
+
+        //computer rotor poses
+        params.rotor_poses.clear();
+        for (uint i = 0; i < 4; i++)
+        {
+            Quaternionr angle(AngleAxisr(arm_angles[i] * M_PIf / 180, unit_z));
+            params.rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(arm_lengths[i], 0, rotor_z), angle, true), unit_z, rotor_directions[i]);
+        };
+
+        //compute inertia matrix
+        computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight);
+    }
+
 private:
     Params params_;
     SensorCollection sensors_; //maintains sensor type indexed collection of sensors

AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp

@@ -449,7 +449,7 @@ class ArduCopterApi : public MultirotorApiBase {
             recv_ret = udpSocket_->recv(&pkt, sizeof(pkt), 100);
         }
 
-        for (auto i = 0; i < RotorControlCount && i < kArduCopterRotorControlCount; ++i) {
+        for (auto i = 0; i < kArduCopterRotorControlCount; ++i) {
             rotor_controls_[i] = pkt.pwm[i];
         }
 
@@ -476,10 +476,7 @@ class ArduCopterApi : public MultirotorApiBase {
     RCData last_rcData_;
     bool is_rc_connected_;
 
-    // TODO: Increase to 6 or 8 for hexa or larger frames, 11 was used in SoloAPI
-    static const int RotorControlCount = 4;
-
-    float rotor_controls_[RotorControlCount];
+    float rotor_controls_[kArduCopterRotorControlCount];
 };
 
 }} //namespace

AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterParams.hpp

@@ -49,37 +49,6 @@ class ArduCopterParams : public MultiRotorParams {
     }
 
 private:
-    void setupFrameGenericQuad(Params& params)
-    {
-        /******* Below is same config as PX4 generic model ********/
-
-        //set up arm lengths
-        //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution
-        params.rotor_count = 4;
-        std::vector<real_T> arm_lengths(params.rotor_count, 0.2275f);
-
-        //set up mass
-        params.mass = 1.0f; //can be varied from 0.800 to 1.600
-        real_T motor_assembly_weight = 0.055f;  //weight for MT2212 motor for F450 frame
-        real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight;
-
-        // using rotor_param default, but if you want to change any of the rotor_params, call calculateMaxThrust() to recompute the max_thrust
-        // given new thrust coefficients, motor max_rpm and propeller diameter.
-        params.rotor_params.calculateMaxThrust();
-
-        //set up dimensions of core body box or abdomen (not including arms).
-        params.body_box.x() = 0.180f; params.body_box.y() = 0.11f; params.body_box.z() = 0.040f;
-        real_T rotor_z = 2.5f / 100;
-
-        //computer rotor poses
-        initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z);
-
-        //compute inertia matrix
-        computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight);
-
-        //leave everything else to defaults
-    }
-
     AirSimSettings::MavLinkConnectionInfo connection_info_;
     std::shared_ptr<const SensorFactory> sensor_factory_;
 };