From 54966174577df3de4cac488a6ed5bfed165abc02 Mon Sep 17 00:00:00 2001 From: Gvalhca Date: Sun, 23 Jun 2019 21:54:33 +0300 Subject: [PATCH 1/4] New architecture --- applications/physics/copter/droneObject.cpp | 388 ++++++++++++++++++ applications/physics/copter/droneObject.h | 87 ++++ applications/physics/copter/motor.cpp | 70 ++++ applications/physics/copter/motor.h | 47 +++ applications/physics/copter/quad.cpp | 35 +- applications/physics/copter/quad.h | 16 +- .../physics/joystick/joystickInterface.cpp | 2 +- applications/physics/mainObject.cpp | 11 +- applications/physics/physMainObject.cpp | 131 ++++++ applications/physics/physMainObject.h | 74 ++++ applications/physics/physObject.cpp | 106 +++++ applications/physics/physObject.h | 133 ++++++ applications/physics/physSphere.cpp | 64 +++ applications/physics/physSphere.h | 22 + applications/physics/physics.pro | 18 +- applications/physics/physics.pro.user | 2 +- applications/physics/physicsMainWindow.cpp | 22 + applications/physics/physicsMainWindow.h | 6 +- applications/physics/simObject.cpp | 22 +- applications/physics/simObject.h | 32 +- core/math/quaternion.h | 1 - 21 files changed, 1242 insertions(+), 47 deletions(-) create mode 100644 applications/physics/copter/droneObject.cpp create mode 100644 applications/physics/copter/droneObject.h create mode 100644 applications/physics/copter/motor.cpp create mode 100644 applications/physics/copter/motor.h create mode 100644 applications/physics/physMainObject.cpp create mode 100644 applications/physics/physMainObject.h create mode 100644 applications/physics/physObject.cpp create mode 100644 applications/physics/physObject.h create mode 100644 applications/physics/physSphere.cpp create mode 100644 applications/physics/physSphere.h diff --git a/applications/physics/copter/droneObject.cpp b/applications/physics/copter/droneObject.cpp new file mode 100644 index 000000000..a66abe3ff --- /dev/null +++ b/applications/physics/copter/droneObject.cpp @@ -0,0 +1,388 @@ +#include "droneObject.h" +#include "core/utils/log.h" + +#include + +using namespace corecvs; + +DroneObject::DroneObject(double frameSize, double mass) +{ + PhysMainObject(); + setSystemMass(mass); + + motors.resize(4, Motor()); + motors[MOTOR_FR].name = "FR"; motors[MOTOR_FR].color = RGBColor::Red(); /*Front right*/ + motors[MOTOR_BL].name = "BL"; motors[MOTOR_BL].color = RGBColor::Green(); /*Back left*/ + motors[MOTOR_FL].name = "FL"; motors[MOTOR_FL].color = RGBColor::Red(); /*Front left*/ + motors[MOTOR_BR].name = "BR"; motors[MOTOR_BR].color = RGBColor::Green(); /*Back right*/ + + double arm = frameSize / 2; + + motors[MOTOR_FR].setPos(Vector3dd( 1, 1, 0).normalised() * arm); motors[MOTOR_FR].cw = true; + motors[MOTOR_BL].setPos(Vector3dd(-1, -1, 0).normalised() * arm); motors[MOTOR_BL].cw = true; + motors[MOTOR_FL].setPos(Vector3dd( 1, -1, 0).normalised() * arm); motors[MOTOR_FL].cw = false; + motors[MOTOR_BR].setPos(Vector3dd(-1, 1, 0).normalised() * arm); motors[MOTOR_BR].cw = false; + + Affine3DQ camPos = Affine3DQ::RotationY(degToRad(90)) * Affine3DQ::RotationZ(degToRad(-90)); + cameras.resize(1); + cameras[0].setLocation(Affine3DQ::Shift(0, 0, 0.02) * camPos); + + PinholeCameraIntrinsics *pinhole = new PinholeCameraIntrinsics(Vector2dd(640, 480), degToRad(60)); + cameras[0].intrinsics.reset(pinhole); + + sensors.resize(1); + sensors[0].position = Affine3DQ::Shift(0, 0, 0.0); + sensors[0].box = AxisAlignedBox3d(Vector3dd(-0.01, -0.01, -0.005), Vector3dd(0.01, 0.01, 0.005) ); + + /* Load ui*/ + MeshLoader loader; + { + Mesh3D mesh; + if (loader.load(&mesh, "models/75mm_whoop_frame_v1.stl")) + { + mesh.transform(Matrix44::Scale(88.0/75.0) * + Matrix44::RotationX(degToRad(-90)) * + Matrix44::Scale(1/1000.0) * + Matrix44::Shift(-23, 0, -23)); + bodyMesh = new Mesh3D; + bodyMesh->add(mesh); + } + } + + { + Mesh3D mesh; + if (loader.load(&mesh, "models/CW_Tri_Prop.stl")) + { + /* mm -> m and shift to right position */ + mesh.transform(Matrix44::Scale(2.0/5.0) * Matrix44::Scale(1/1000.0) * Matrix44::Shift(-236, -207, 0)); + + motors[MOTOR_FR].propMesh = new Mesh3D; + motors[MOTOR_FR].propMesh->add(mesh); + + motors[MOTOR_BL].propMesh = new Mesh3D; + motors[MOTOR_BL].propMesh->add(mesh); + } + } + + { + Mesh3D mesh; + if (loader.load(&mesh, "models/CCW_Tri_Prop.stl")) + { + /* mm -> m and shift to right position */ + mesh.transform(Matrix44::Scale(2.0/5.0) * Matrix44::Scale(1/1000.0) * Matrix44::Shift(-372, -208, 0)); + + motors[MOTOR_BR].propMesh = new Mesh3D; + motors[MOTOR_BR].propMesh->add(mesh); + + motors[MOTOR_FL].propMesh = new Mesh3D; + motors[MOTOR_FL].propMesh->add(mesh); + } + } + + { + Mesh3DDecorated mesh; + if (loader.load(&mesh, "models/OBJ.obj")) + { + /* mm -> m and shift to right position */ + worldMesh = new Mesh3DDecorated; + mesh.transform(Matrix44::RotationZ(degToRad(90)) * + Matrix44::Shift(0, -20, -4) * + Matrix44::Scale(1/200.0) * + Matrix44::RotationX(degToRad(90))); + + worldMesh->add(mesh, true); + worldMesh->recomputeMeanNormals(); + + worldMesh->dumpInfo(); + } + } + + double massOfCentralSphere = mass - 4 * motors[0].mass; + Affine3DQ posOfCentralSphere = Affine3DQ(Vector3dd(0,0,0).normalised()); + double radiusOfCentralSphere = arm / 2; + centralSphere = PhysSphere(&posOfCentralSphere, &radiusOfCentralSphere, &massOfCentralSphere); + //motors.insert(motors.end(), motors.begin(), motors.end()); + + objects.push_back(¢ralSphere); + //objects.push_back(&motors[0]); + + for (size_t i = 0; i < motors.size(); ++i) + { + objects.push_back(&motors[i]); + } +} + +void DroneObject::tick(double deltaT) +{ + double radius = centralSphere.radius; + double motorMass = objects[1]->mass; + double centerMass = objects[0]->mass; + double arm = objects[2]->getPosVector().l2Metric(); + double inertialMomentX = 2.0 / 5.0 * centerMass * pow(radius, 2) + 2 * motorMass * pow(arm, 2); + double inertialMomentY = 2.0 / 5.0 * centerMass * pow(radius, 2) + 2 * motorMass * pow(arm, 2); + double inertialMomentZ = 2.0 / 5.0 * centerMass * pow(radius, 2) + 4 * motorMass * pow(arm, 2); + + inertiaTensor = Matrix33(inertialMomentX, 0, 0, + 0, inertialMomentY, 0, + 0, 0, inertialMomentZ); + + + calcForce(); + calcMoment(); + setPosCenter(getPosCenter() + velocity * deltaT); + velocity += (getForce() / getSystemMass()) * deltaT; + + /* We should carefully use inertiaTensor here. It seems like it changes with the frame of reference */ + Vector3dd W = inertiaTensor.inv() * getMomentum(); + Quaternion angularAcceleration = Quaternion::Rotation(W, W.l2Metric()); + + Quaternion q = orientation; + orientation = Quaternion::pow(angularVelocity, deltaT) ^ orientation; + + //orientation.printAxisAndAngle(); + angularVelocity = Quaternion::pow(angularAcceleration, deltaT) ^ angularVelocity; + L_INFO<<"Delta orient: "<drawMesh(mesh) passed")); + /** Very bad kludge, can be used only as a last resort **/ + /* + if (Motor* motor = dynamic_cast(this)) + motor->drawMyself(mesh); + else + L_INFO << "dynamic_cast at droneObject.drawMyself() unexpectedly failed"; + */ + mesh.popTransform(); + } +} + +void DroneObject::drawCameras(Mesh3D &mesh) +{ + for (size_t i = 0; i < cameras.size(); i++) + { + CalibrationDrawHelpers helper; + helper.setSolidCameras(true); + helper.drawCamera(mesh, cameras[i], 0.01); + } + +} + +void DroneObject::drawSensors(Mesh3D &mesh) +{ + for (size_t i = 0; i < sensors.size(); i++) + { + mesh.mulTransform(sensors[i].position); + mesh.addAOB(sensors[i].box); + mesh.popTransform(); + } +} + +void DroneObject::drawForces(Mesh3D &mesh) +{ + for (size_t i = 0; i < motors.size(); i++) + { + Affine3DQ motorToWorld = getTransform() * motors[i].getPosAffine(); + Vector3dd f = motorToWorld * motors[i].getForce(); + mesh.addLine(motors[i].getPosVector(), motors[i].getPosVector() + f * 1.0); + } +} + + + +Affine3DQ DroneObject::getTransform() +{ + return Affine3DQ(this->orientation, this->getPosCenter()); +} + +void DroneObject::drawMyself(Mesh3DDecorated &mesh) +{ + DroneObject::drawMyself((Mesh3D &)mesh); + /* Scene should be drawed */ + + //SYNC_PRINT(("Quad::drawMyself(Mesh3DDecorated &mesh): before\n")); + //mesh.dumpInfo(); + if (worldMesh != NULL) + { + mesh.add(*worldMesh); + } + //SYNC_PRINT(("Quad::drawMyself(Mesh3DDecorated &mesh): after\n")); + //mesh.dumpInfo(); + + +} + +Vector3dd DroneObject::FromQuaternion(Quaternion &Q) +{ + double yaw = asin (2.0 * (Q.t() * Q.y() - Q.z() * Q.x())); + double pitch = atan2 (2.0 * (Q.t() * Q.x() + Q.y() * Q.z()),1.0 - 2.0 * (Q.x() * Q.x() + Q.y() * Q.y())); + double roll = atan2 (2.0 * (Q.t() * Q.z() + Q.x() * Q.y()),1.0 - 2.0 * (Q.y() * Q.y() + Q.z() * Q.z())); + return Vector3dd(pitch, roll, yaw); +} + +PID::PID(double p, double i, double d) +{ + P=p; + I=i; + D=d; +} + +void DroneObject::flightControllerTick(const CopterInputs &input) +{ + /* Mixer */ + double throttle = (input.axis[CopterInputs::CHANNEL_THROTTLE] - 1500) / 12; + double wantedPitch = (input.axis[CopterInputs::CHANNEL_PITCH] - 1500) / 12; + double wantedYaw = (input.axis[CopterInputs::CHANNEL_YAW] - 1500) / 12; + double wantedRoll = (input.axis[CopterInputs::CHANNEL_ROLL] - 1500) / 12; + + /** Get current Pitch, Roll, Yaw of drone at this moment **/ + Quaternion q = angularVelocity; + Vector3dd currentPRY = FromQuaternion(angularVelocity); + + for (int i = 0; i < 3; i++) + { + if(currentPRY[i]<0.001) + { + currentPRY[i]=0; + } + } + + /** Get current between PRY now and wanted **/ + Vector3dd currentError(wantedPitch - currentPRY.x(), + wantedRoll - currentPRY.y(), + wantedRoll - currentPRY.z()); + + pitchPID.sumOfError+=currentError.x(); + rollPID.sumOfError+=currentError.y(); + yawPID.sumOfError+=currentError.z(); + + double forceP, forceR, forceY; + double deltaT=0.1; + + forceP = pitchPID.P * currentError.x() + + pitchPID.I * deltaT * pitchPID.sumOfError + + pitchPID.D * (currentError.x() - pitchPID.prevError) / deltaT; + + forceR = rollPID.P * currentError.y() + + rollPID.I * deltaT * rollPID.sumOfError + + rollPID.D * (currentError.y() - rollPID.prevError) / deltaT; + + forceY = yawPID.P * currentError.z() + + yawPID.I * deltaT * yawPID.sumOfError + + yawPID.D * (currentError.z() - yawPID.prevError) / deltaT; + + pitchPID.prevError = currentError.x(); + rollPID.prevError = currentError.y(); + yawPID.prevError = currentError.z(); + + + motors[MOTOR_FR].pwm = - wantedPitch + wantedRoll + wantedYaw + throttle; + motors[MOTOR_BL].pwm = wantedPitch + -wantedRoll + wantedYaw + throttle; + motors[MOTOR_FL].pwm = - wantedPitch + -wantedRoll - wantedYaw + throttle; + motors[MOTOR_BR].pwm = wantedPitch + wantedRoll - wantedYaw + throttle; + +/* + motors[MOTOR_FR].pwm = - forceP + forceR + forceY + throttle; + motors[MOTOR_BL].pwm = forceP - forceR + forceY + throttle; + motors[MOTOR_FL].pwm = - forceP - forceR - forceY + throttle; + motors[MOTOR_BR].pwm = forceP + forceR - forceY + throttle; +*/ + //L_INFO<<"PitchPID P: "< 1.0) motors[i].pwm = 1.0; + } + + //L_INFO<<"Motor True Values: "< +#include +#include "motor.h" + +#include +#include "core/cameracalibration/calibrationDrawHelpers.h" +#include "core/geometry/mesh3d.h" +#include "core/math/affine.h" +#include "core/math/vector/vector3d.h" + +#include "physMainObject.h" + + +class PID +{ +public: + double P,I,D; + double prevError = 0.0; + double sumOfError = 0.0; + PID(double p, double i, double d); +}; + + +class Sensor +{ +public: + corecvs::Affine3DQ position; + AxisAlignedBox3d box; + std::string name; +}; + +class DroneObject : public PhysMainObject +{ +public: + std::vector cameras; + std::vector sensors; + std::vector motors; + PhysSphere centralSphere; + + PID pitchPID{0.7, 0.35, 0.35}; + PID rollPID{0.7, 0.35, 0.35}; + PID yawPID{0.7, 0.35, 0.35}; + + enum BetaflightMotors { + BETAFLIGHT_MOTOR_1 = 0, + BETAFLIGHT_MOTOR_2 = 1, + BETAFLIGHT_MOTOR_3 = 2, + BETAFLIGHT_MOTOR_4 = 3, + + MOTOR_BR = BETAFLIGHT_MOTOR_1, + MOTOR_FR = BETAFLIGHT_MOTOR_2, + MOTOR_BL = BETAFLIGHT_MOTOR_3, + MOTOR_FL = BETAFLIGHT_MOTOR_4, + }; + + DroneObject(double frameSize = 0.088, double mass = 0.12); + + virtual void tick(double deltaT) override; + + Vector3dd FromQuaternion(Quaternion &Q); + corecvs::Affine3DQ getTransform(); + void drawMyself(Mesh3D &mesh); + void drawMyself(Mesh3DDecorated &mesh); + void flightControllerTick(const CopterInputs &input); + void visualTick(); + void physicsTick(); + + /* UI */ + Mesh3D *bodyMesh = NULL; + + /* This definitely need to be in the other place */ + Mesh3DDecorated *worldMesh = NULL; + + ~DroneObject(); +private: + void drawForces(Mesh3D &mesh); + void drawSensors(Mesh3D &mesh); + void drawCameras(Mesh3D &mesh); + void drawMotors(Mesh3D &mesh); + void drawBody(Mesh3D &mesh); + //Vector3dd getForceTransformed(const Affine3DQ &T, Motor &obj); +}; + +#endif // DRONEOBJECT_H diff --git a/applications/physics/copter/motor.cpp b/applications/physics/copter/motor.cpp new file mode 100644 index 000000000..02ba3b7ae --- /dev/null +++ b/applications/physics/copter/motor.cpp @@ -0,0 +1,70 @@ +#include "motor.h" + +Motor::Motor() +{ + cw = false; + motorWidth = 0.011; /**< in m **/ + motorHeight = 0.004; /**< in m **/ + double propellerRadius = 0.020; /**< propeller radius in m **/ + double motorMass = 0.01; /**< in kg **/ + Affine3DQ defaultPos = Affine3DQ(Vector3dd::Zero()); + + /** Creating PhysSphere with propeller radius and mass of motor **/ + PhysSphere(&defaultPos, &propellerRadius, &motorMass); +} + +void Motor::calcMoment() +{ + addMoment(force * getPosVector()); + Vector3dd v = calcMotorMoment(); + L_INFO << "Motor::calcMoment() was called. Moment of current motor is: " << v; +} + +void Motor::drawMesh(corecvs::Mesh3D &mesh) +{ + if(mesh.hasColor) { + mesh.setColor(color); + } + + if (motorMesh != NULL) { + mesh.add(*motorMesh); + } else { + mesh.addCylinder(Vector3dd::Zero(), motorWidth / 2, motorHeight, 10, 0); + } + mesh.mulTransform(Affine3DQ::RotationZ(phi)); + + if (propMesh != NULL) { + mesh.add(*propMesh); + } else { + mesh.addAOB(Vector3dd(-radius, -0.002 , motorHeight ), + Vector3dd(+radius, 0.002 , motorHeight + 0.002)); + } + mesh.popTransform(); + + SYNC_PRINT(("Successfully drew motor mesh")); +} + +Vector3dd Motor::getForce() +{ + return force; +} + +void Motor::calcForce() +{ + force = Vector3dd(0, 0, maxForce * pwm); +} + +Vector3dd Motor::getForceTransformed(const Affine3DQ &T) +{ + return T.rotor * getForce(); +} + +Vector3dd Motor::calcMotorMoment() +{ + double k = 2 * pow(10, -6), b = 1 * pow(10, -7); + double momentum = pwm / k * b * (cw ? 1: -1); + //if(momentum!=0) + //L_INFO<<"Momentum of "< +#include + +#include +#include "core/cameracalibration/calibrationDrawHelpers.h" +#include "core/geometry/mesh3d.h" +#include "core/math/affine.h" +#include "core/math/vector/vector3d.h" + +#include "physSphere.h" + +class Motor : public PhysSphere +{ +public: + /** + * By design this is an encapsulated motor class, it knows nothing about how it is mounted + **/ + /* Configuration */ + bool cw; + double maxForce = 9.8 / 3; /* Each motor is capable of just lifing itself */ + + double motorWidth; /**< in m **/ + double motorHeight; /**< in m **/ + + std::string name = "-"; + corecvs::RGBColor color = corecvs::RGBColor::Red(); + + /* State */ + double pwm = 0.0; /**< 0..1 **/ + double phi = degToRad(32); + virtual void calcMoment() override; + virtual void drawMesh(corecvs::Mesh3D &mesh) override; + Vector3dd getForce(); + void calcForce(); + Vector3dd getForceTransformed(const Affine3DQ &T); + Vector3dd calcMotorMoment(); + + /* UI not owned. Need to be reworked. Could be reused for hardcoded solution */ + Mesh3D *motorMesh = NULL; + Mesh3D *propMesh = NULL; + Motor(); +}; + +#endif // MOTOR_H diff --git a/applications/physics/copter/quad.cpp b/applications/physics/copter/quad.cpp index 76efd4209..34bb787e8 100644 --- a/applications/physics/copter/quad.cpp +++ b/applications/physics/copter/quad.cpp @@ -8,7 +8,8 @@ using namespace corecvs; Quad::Quad(double frameSize) { position = Vector3dd::Zero(); - mass = 0.2; /* 200g copter */ + /** IF changed then need to go to SimObject::getM()**/ + mass = 0.12; /* 120g copter */ motors.resize(4); motors[BETAFLIGHT_MOTOR_2].name = "FR"; motors[BETAFLIGHT_MOTOR_2].color = RGBColor::Red(); /*Front right*/ @@ -23,7 +24,6 @@ Quad::Quad(double frameSize) motors[BETAFLIGHT_MOTOR_4].position.shift = Vector3dd( 1, -1, 0).normalised() * arm; motors[BETAFLIGHT_MOTOR_4].cw = false; motors[BETAFLIGHT_MOTOR_1].position.shift = Vector3dd(-1, 1, 0).normalised() * arm; motors[BETAFLIGHT_MOTOR_1].cw = false; - Affine3DQ camPos = Affine3DQ::RotationY(degToRad(90)) * Affine3DQ::RotationZ(degToRad(-90)); cameras.resize(1); cameras[0].setLocation(Affine3DQ::Shift(0, 0, 0.02) * camPos); @@ -197,10 +197,10 @@ PID::PID(double p, double i, double d) void Quad::flightControllerTick(const CopterInputs &input) { /* Mixer */ - double throttle = (input.axis[CopterInputs::CHANNEL_THROTTLE] - 1500) / 12; - double wantedPitch = (input.axis[CopterInputs::CHANNEL_PITCH] - 1500) / 12; - double wantedYaw = (input.axis[CopterInputs::CHANNEL_YAW] - 1500) / 12; - double wantedRoll = (input.axis[CopterInputs::CHANNEL_ROLL] - 1500) / 12; + double throttle = (input.axis[CopterInputs::CHANNEL_THROTTLE] - 1500) / 12; + double wantedPitch = (input.axis[CopterInputs::CHANNEL_PITCH] - 1500) / 12; + double wantedYaw = (input.axis[CopterInputs::CHANNEL_YAW] - 1500) / 12; + double wantedRoll = (input.axis[CopterInputs::CHANNEL_ROLL] - 1500) / 12; /** Get current Pitch, Roll, Yaw of drone at this moment **/ Quaternion q = angularVelocity; @@ -242,22 +242,25 @@ void Quad::flightControllerTick(const CopterInputs &input) rollPID.prevError = currentError.y(); yawPID.prevError = currentError.z(); - /* - motors[BETAFLIGHT_MOTOR_2].pwm = - pitch + roll + yaw + throttle; - motors[BETAFLIGHT_MOTOR_3].pwm = pitch + -roll + yaw + throttle; - motors[BETAFLIGHT_MOTOR_4].pwm = - pitch + -roll - yaw + throttle; - motors[BETAFLIGHT_MOTOR_1].pwm = pitch + roll - yaw + throttle; - */ + motors[BETAFLIGHT_MOTOR_2].pwm = - wantedPitch + wantedRoll + wantedYaw + throttle; + motors[BETAFLIGHT_MOTOR_3].pwm = wantedPitch + -wantedRoll + wantedYaw + throttle; + motors[BETAFLIGHT_MOTOR_4].pwm = - wantedPitch + -wantedRoll - wantedYaw + throttle; + motors[BETAFLIGHT_MOTOR_1].pwm = wantedPitch + wantedRoll - wantedYaw + throttle; + +/* motors[BETAFLIGHT_MOTOR_2].pwm = - forceP + forceR + forceY + throttle; motors[BETAFLIGHT_MOTOR_3].pwm = forceP - forceR + forceY + throttle; motors[BETAFLIGHT_MOTOR_4].pwm = - forceP - forceR - forceY + throttle; motors[BETAFLIGHT_MOTOR_1].pwm = forceP + forceR - forceY + throttle; - +*/ //L_INFO<<"PitchPID P: "< 1.0) motors[i].pwm = 1.0; } + //L_INFO<<"Motor True Values: "< #include #include - #include "core/cameracalibration/calibrationDrawHelpers.h" #include "core/geometry/mesh3d.h" #include "core/math/affine.h" @@ -37,8 +35,6 @@ class Motor **/ corecvs::Affine3DQ position; - - /* Configuration */ bool cw = false; double maxForce = 9.8 / 3; /* Each motor is capable of just lifing itself */ @@ -54,7 +50,6 @@ class Motor double pwm = 0.0; /**< 0..1 **/ double phi = degToRad(32); - void drawMyself(corecvs::Mesh3D &mesh) { if(mesh.hasColor) { @@ -86,18 +81,18 @@ class Motor Vector3dd getM() { - return Vector3dd(0.0, 0.0, pwm * cw); + double k = 2 * pow(10, -6), b = 1 * pow(10, -7); + double momentum = pwm / k * b * (cw ? 1: -1); + //if(momentum!=0) + //L_INFO<<"Momentum of "< JoystickInterface::getDevices(const string &prefix) } -JoystickConfiguration JoystickInterface::getConfiguration(int joystickDevice) +JoystickConfiguration JoystickInterface::getConfiguration(int joystickDevice) { JoystickConfiguration toReturn; diff --git a/applications/physics/mainObject.cpp b/applications/physics/mainObject.cpp index e06138042..170a14d1b 100644 --- a/applications/physics/mainObject.cpp +++ b/applications/physics/mainObject.cpp @@ -47,7 +47,16 @@ void MainObject::setCenterOfMass() } massCenter = massCenter / systemMass; } - +/* +void MainObject::calcForce() +{ + Vector3dd f = Vector3dd::Zero(); + for (size_t i = 0; i < objects.size(); ++i) + { + f += objects[i]-> + } +} +*/ void MainObject::addObject(SimObject *object) { objects.push_back(object); diff --git a/applications/physics/physMainObject.cpp b/applications/physics/physMainObject.cpp new file mode 100644 index 000000000..0128e4351 --- /dev/null +++ b/applications/physics/physMainObject.cpp @@ -0,0 +1,131 @@ +#include "physMainObject.h" + + +PhysMainObject::PhysMainObject() +{ + posCenter = Vector3dd(1,1,1); + systemMass = 0; + force = Vector3dd::Zero(); + //moment = Vector3dd::Zero(); + + L_INFO << "default PhysMainObject():created"; +} + +void PhysMainObject::addForce(const corecvs::Vector3dd &_force) +{ + for (size_t i = 0; i < objects.size(); ++i) { + objects[i]->addForce(_force); + } + calcForce(); +} + +void PhysMainObject::tick(double deltaT) +{ + SYNC_PRINT(("PhysMainObject::tick should not be called")); +} + +void PhysMainObject::calcCenterOfMass() +{ + for (size_t i = 0; i < objects.size(); ++i) + { + massCenter += objects[i]->mass * objects[i]->getPosVector(); + systemMass += objects[i]->mass; + } + massCenter = massCenter / systemMass; +} + +const Vector3dd PhysMainObject::getCenterOfMass() +{ + calcCenterOfMass(); + return massCenter; +} + +void PhysMainObject::setForce(const Vector3dd &_force) +{ + force = _force; +} + +const corecvs::Vector3dd PhysMainObject::getForce() +{ + return force; +} + +void PhysMainObject::setMomentum(const Vector3dd &_m) +{ + momentum = _m; +} + +const Vector3dd PhysMainObject::getMomentum() +{ + return momentum; +} + +void PhysMainObject::setSystemMass(const double m) +{ + systemMass = m; +} + +double PhysMainObject::getSystemMass() const +{ + return systemMass; +} + + +void PhysMainObject::calcForce() +{ + Vector3dd f = Vector3dd::Zero(); + for(size_t i = 0; i < objects.size(); ++i) + { + f += objects[i]->getForce(); + } + setForce(f); +} + +void PhysMainObject::calcMoment() +{ + Vector3dd m = Vector3dd::Zero(); + for(size_t i = 0; i < objects.size(); ++i) + { + objects[i]->calcMoment(); + m += objects[i]->getMoment(); + } + setMomentum(m); +} + +void PhysMainObject::setPosCenter(const Vector3dd &_pos) +{ + posCenter = _pos; +} + +const Vector3dd PhysMainObject::getPosCenter() +{ + return posCenter; +} + +void PhysMainObject::addObject(PhysObject *object) +{ + objects.push_back(object); + calcCenterOfMass(); +} + +/* +void PhysMainObject::addSphere(Vector3dd coords, double radius) +{ + objects.push_back(new PhysSphere(coords,radius)); + setCenterOfMass(); +} + +void PhysMainObject::addSphere(Vector3dd coords, double radius,corecvs::RGBColor color) +{ + objects.push_back(new SimSphere(coords,radius,color)); + setCenterOfMass(); +} +*/ +PhysMainObject::~PhysMainObject() +{ + for (PhysObject *obj : objects) + { + delete_safe(obj); + } + objects.clear(); +} diff --git a/applications/physics/physMainObject.h b/applications/physics/physMainObject.h new file mode 100644 index 000000000..41b5628dc --- /dev/null +++ b/applications/physics/physMainObject.h @@ -0,0 +1,74 @@ +#ifndef PHYSMAINOBJECT_H +#define PHYSMAINOBJECT_H + +#include "bits/stdc++.h" +#include "list" +#include "iostream" +#include "physObject.h" +#include "physSphere.h" + +#include + +class PhysMainObject +{ +private: + Vector3dd force = Vector3dd::Zero(); + Vector3dd oldForce = Vector3dd::Zero(); + Vector3dd massCenter = Vector3dd::Zero(); + Vector3dd posCenter; + Vector3dd momentum = Vector3dd::Zero(); + double systemMass; + +public: + PhysMainObject(); + + /** Owned and manitained by PhysMainObject**/ + vector objects; + + Vector3dd velocity = Vector3dd::Zero(); + int frameCounter = 0; + + Quaternion orientation = Quaternion::Identity(); + + Quaternion angularVelocity = Quaternion::Identity(); + + /** Mass in kilograms **/ + bool countPhysics = false; + corecvs::Matrix33 inertiaTensor = corecvs::Matrix33::Identity(); + + void addForce (const Vector3dd &force); + void addImpulse(const Vector3dd &force); + void calcCenterOfMass(); + const Vector3dd getCenterOfMass(); + + /** Get & Set **/ + void setForce(const Vector3dd &_force); + const Vector3dd getForce(); + void setMomentum(const Vector3dd &_m); + const Vector3dd getMomentum(); + void setSystemMass(const double m); + double getSystemMass() const; + + void calcForce(); + void calcMoment(); + + void setPosCenter(const Vector3dd &_pos); + const Vector3dd getPosCenter(); + + + virtual void tick(double deltaT); + + void addSphere(Vector3dd coords, double radius); + void addSphere(Vector3dd coords, double radius, corecvs::RGBColor); + + /** + * Adds subobject to MainObject and transfers ownership + **/ + void addObject(PhysObject *object); + + virtual ~PhysMainObject(); + }; + + + +#endif // PHYSMAINOBJECT_H diff --git a/applications/physics/physObject.cpp b/applications/physics/physObject.cpp new file mode 100644 index 000000000..844ed26eb --- /dev/null +++ b/applications/physics/physObject.cpp @@ -0,0 +1,106 @@ +#include "physObject.h" + +#include + +#include "core/utils/utils.h" +#include "core/geometry/mesh3d.h" +#include "core/geometry/mesh3DDecorated.h" + +using namespace std; +using namespace corecvs; + + +void PhysObject::startTick() +{ + F = Vector3dd::Zero(); + M = Vector3dd::Zero(); +} + +void PhysObject::addForce(const Vector3dd &force) +{ + F += force; + //M += force.getM(); +} + +void PhysObject::addMoment(const Vector3dd &moment) +{ + M += moment; +} + +void PhysObject::calcMoment() +{ + M = force * getPosVector(); + SYNC_PRINT(("PhysObject::calcMoment() was called")); +} + +void PhysObject::tick(double deltaT) +{ + +} + +const corecvs::Affine3DQ PhysObject::getPosAffine() const +{ + return position; +} + +const Vector3dd PhysObject::getPosVector() const +{ + return position.shift; +} + +Vector3dd PhysObject::getForce() const +{ + return F; +} + +Vector3dd PhysObject::getMoment() const +{ + return M; +} + +void PhysObject::addToMesh(Mesh3D &mesh) +{ + SYNC_PRINT(("Don't know how to do this")); +} + +void PhysObject::drawMesh(Mesh3D &mesh) +{ + SYNC_PRINT(("PhysObject can't draw mesh")); +} + +void PhysObject::setPosition(const Affine3DQ &pos) +{ + this->position = pos; +} + +void PhysObject::setPosition(const Vector3dd &pos) +{ + this->position = Affine3DQ(pos); +} + +PhysObject::PhysObject() +{ + position = Affine3DQ(Vector3dd(1,1,1)); + mass = 1; + + F = Vector3dd::Zero(); + M = Vector3dd::Zero(); + + L_INFO << "default PhysObject():created"; +} + +PhysObject::PhysObject(const Affine3DQ *coords, const double *m) +{ + position = Affine3DQ(*coords); + mass = *m; + + F = Vector3dd::Zero(); + M = Vector3dd::Zero(); + + L_INFO << "PhysObject():created with " << mass << " kg mass and at " << position << " position"; +} + +void PhysObject::saveMesh(const std::string &/*name*/) +{ + cout << "PhysObject::saveMesh(): not here" << endl; +} diff --git a/applications/physics/physObject.h b/applications/physics/physObject.h new file mode 100644 index 000000000..dd8ca2bad --- /dev/null +++ b/applications/physics/physObject.h @@ -0,0 +1,133 @@ +#ifndef PHYSOBJECT_H +#define PHYSOBJECT_H + +#include +#include +#include "core/utils/log.h" +#include +#include +#include "core/utils/utils.h" +#include "core/geometry/mesh3d.h" +#include "core/math/matrix/matrix33.h" +#include "core/geometry/mesh3DDecorated.h" +#include "core/math/affine.h" + +/** Temporary usages while we are preparing to move this to the proper place **/ +using corecvs::Vector3dd; +using corecvs::Quaternion; + +/* +class Force { +public: + corecvs::Vector3dd force = corecvs::Vector3dd::Zero(); + //corecvs::Vector3dd position = corecvs::Vector3dd::Zero(); + //corecvs::Vector3dd centerPos = corecvs::Vector3dd::Zero(); + + Force() {} + + Force(double fx, double fy, double fz) : + force(fx, fy, fz) + { + } + + Force(Vector3dd force) : + force(force) + { + } + + void transform(const corecvs::Affine3DQ &T) + { + force = T.rotor * force; + position = T * position; + + //L_INFO<<"Force Pos: "<< position; + //L_INFO<<"Center Pos: "< + +PhysSphere::PhysSphere() +{ + PhysObject(); + radius = 1.0; +} + +void PhysSphere::setPos(const Vector3dd &pos) +{ + this->setPosition(Affine3DQ(pos)); +} + +PhysSphere::PhysSphere(Affine3DQ *pos, double *r, double *m) +{ + PhysObject(pos, m); + radius = *r; +} + +void PhysSphere::saveMesh(const std::string &name) +{ + cout<<"here"<setMesh(mesh); + + mesh->switchColor(); + mesh->mulTransform(copterPos); + mesh->setColor(RGBColor::Red()); + mesh->addIcoSphere(getPosVector(), 2, 2); + mesh->popTransform(); + + //mesh->dumpPLY(name+".ply"); + delete_safe(mesh); +} + +void PhysSphere::drawMesh() +{ + //mesh is not using because I'm a little bit genius (really don't know how to make best architecture by now) + Mesh3D *sphereMesh = new Mesh3D; + corecvs::RGBColor color = corecvs::RGBColor(255, 255, 0); + sphereMesh->switchColor(); + + //Should be fixed with gathering coordinates of physMainObject or mesh should be transfering inside physMainObject + sphereMesh->mulTransform(Affine3DQ(Vector3dd::Zero())); + + sphereMesh->setColor(color); + sphereMesh->addIcoSphere(getPosVector(), 2, 2); + sphereMesh->popTransform(); + + //mesh->dumpPLY(name+".ply"); + delete_safe(sphereMesh); + SYNC_PRINT(("Successfully drew sphere mesh")); +} + +void PhysSphere::addToMesh(Mesh3D &mesh) +{ + mesh.addIcoSphere(getPosVector(), radius, 3); +} diff --git a/applications/physics/physSphere.h b/applications/physics/physSphere.h new file mode 100644 index 000000000..4420cdb08 --- /dev/null +++ b/applications/physics/physSphere.h @@ -0,0 +1,22 @@ +#ifndef PHYSSPHERE_H +#define PHYSSPHERE_H + +#include "physObject.h" +using namespace corecvs; +class PhysSphere : public PhysObject +{ +public: + PhysSphere(); + PhysSphere(corecvs::Affine3DQ *pos, double *r, double *m); + + /*Get & Set*/ + void setPos(const Vector3dd &pos); + + virtual void addToMesh (corecvs::Mesh3D &mesh) override; + virtual void saveMesh(const std::string &name) override; + void drawMesh(); + + double radius; +}; + +#endif // PHYSSPHERE_H diff --git a/applications/physics/physics.pro b/applications/physics/physics.pro index be4e153d1..e2de22c03 100644 --- a/applications/physics/physics.pro +++ b/applications/physics/physics.pro @@ -24,6 +24,7 @@ INCLUDEPATH += mixer HEADERS += \ + physMainObject.h \ simulation.h \ clientSender.h \ joystickInput.h \ @@ -41,11 +42,16 @@ HEADERS += \ joystick/mixerChannelOperationWidget.h \ frameProcessor.h \ protoautopilot.h \ - copter/quad.h \ + #copter/quad.h \ physicsMainWindow.h \ - physicsAboutWidget.h + physicsAboutWidget.h \ + physObject.h \ + physSphere.h \ + copter/droneObject.h \ + copter/motor.h SOURCES += \ + physMainObject.cpp \ simulation.cpp \ simSphere.cpp \ simObject.cpp \ @@ -64,9 +70,13 @@ SOURCES += \ joystick/mixerChannelOperationWidget.cpp \ frameProcessor.cpp \ protoautopilot.cpp \ - copter/quad.cpp \ + #copter/quad.cpp \ physicsMainWindow.cpp \ - physicsAboutWidget.cpp + physicsAboutWidget.cpp \ + physObject.cpp \ + physSphere.cpp \ + copter/droneObject.cpp \ + copter/motor.cpp FORMS += \ joystick/JoystickOptionsWidget.ui \ diff --git a/applications/physics/physics.pro.user b/applications/physics/physics.pro.user index d24817ad1..2a5f54273 100644 --- a/applications/physics/physics.pro.user +++ b/applications/physics/physics.pro.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/applications/physics/physicsMainWindow.cpp b/applications/physics/physicsMainWindow.cpp index af5249218..0b87fad79 100644 --- a/applications/physics/physicsMainWindow.cpp +++ b/applications/physics/physicsMainWindow.cpp @@ -569,6 +569,7 @@ void PhysicsMainWindow::mainAction() */ //startJoyStickMode(); + /** copter.flightControllerTick(joystick1.output); copter.physicsTick(); @@ -588,6 +589,27 @@ void PhysicsMainWindow::mainAction() mGraphDialog.addGraphPoint("Y", copter.position.y()); mGraphDialog.addGraphPoint("Z", copter.position.z()); + mGraphDialog.update(); + **/ + drone.flightControllerTick(joystick1.output); + drone.physicsTick(); + + drone.visualTick(); + + if (oldbackend) { + drone.drawMyself(*scene->owned); + } else { + Mesh3DDecorated *mesh = new Mesh3DDecorated(); + mesh->switchNormals(); + drone.drawMyself(*mesh); + //mesh->dumpInfo(); + mShadedScene->setMesh(mesh); + } + + mGraphDialog.addGraphPoint("X", drone.getPosCenter().x()); + mGraphDialog.addGraphPoint("Y", drone.getPosCenter().y()); + mGraphDialog.addGraphPoint("Z", drone.getPosCenter().z()); + mGraphDialog.update(); if (oldbackend) { diff --git a/applications/physics/physicsMainWindow.h b/applications/physics/physicsMainWindow.h index 5107ad06c..c48c1a0de 100644 --- a/applications/physics/physicsMainWindow.h +++ b/applications/physics/physicsMainWindow.h @@ -19,7 +19,8 @@ #include #include -#include +//#include +#include #include "clientSender.h" #include "copterInputsWidget.h" @@ -124,7 +125,8 @@ public slots: ControlsMixer mixer; CopterInputs inputs; QTimer copterTimer; - Quad copter; + //Quad copter; + DroneObject drone; public slots: /* Let it be here so far */ diff --git a/applications/physics/simObject.cpp b/applications/physics/simObject.cpp index a6c722f6f..215176961 100644 --- a/applications/physics/simObject.cpp +++ b/applications/physics/simObject.cpp @@ -18,7 +18,7 @@ void SimObject::startTick() void SimObject::addForce(const Force &force) { - F += force.netForce(); + F += force.getForce(); M += force.getM(); } @@ -28,18 +28,32 @@ void SimObject::addMoment(const Vector3dd &moment) } void SimObject::tick(double deltaT) -{ +{ + /** KILLME **/ + double radius = 0.050; + double motorMass = 0.01; + double frameSize = 0.088; + double centerMass = 0.08; + double inertialMomentX = 2.0 / 5.0 * centerMass * pow(radius, 2) + 2 * motorMass * pow(frameSize / 2, 2); + double inertialMomentY = 2.0 / 5.0 * centerMass * pow(radius, 2) + 2 * motorMass * pow(frameSize / 2, 2); + double inertialMomentZ = 2.0 / 5.0 * centerMass * pow(radius, 2) + 4 * motorMass * pow(frameSize / 2, 2); + inertiaTensor = Matrix33(inertialMomentX, 0, 0, + 0, inertialMomentY, 0, + 0, 0, inertialMomentZ); + position += velocity * deltaT; velocity += (F / mass) * deltaT; /* We should carefully use inertiaTensor here. It seems like it changes with the frame of reference */ - Vector3dd W = inertiaTensor.inv() * M; Quaternion angularAcceleration = Quaternion::Rotation(W, W.l2Metric()); + Quaternion q = orientation; orientation = Quaternion::pow(angularVelocity , deltaT) ^ orientation; - angularVelocity = Quaternion::pow(angularAcceleration, deltaT) ^ angularVelocity; + //orientation.printAxisAndAngle(); + angularVelocity = Quaternion::pow(angularAcceleration, deltaT) ^ angularVelocity; + L_INFO<<"Delta orient: "< #include - +#include "core/utils/log.h" #include - +#include #include "core/utils/utils.h" #include "core/geometry/mesh3d.h" #include "core/math/matrix/matrix33.h" @@ -18,6 +19,7 @@ class Force { public: corecvs::Vector3dd force = corecvs::Vector3dd::Zero(); corecvs::Vector3dd position = corecvs::Vector3dd::Zero(); + corecvs::Vector3dd centerPos = corecvs::Vector3dd::Zero(); Force() {} @@ -35,6 +37,9 @@ class Force { { force = T.rotor * force; position = T * position; + + //L_INFO<<"Force Pos: "<< position; + //L_INFO<<"Center Pos: "<, x = Q1.t() * Q2.x() + Q1.x() * Q2.t() + Q1.y() * Q2.z() - Q1.z() * Q2.y(); y = Q1.t() * Q2.y() - Q1.x() * Q2.z() + Q1.y() * Q2.t() + Q1.z() * Q2.x(); z = Q1.t() * Q2.z() + Q1.x() * Q2.y() - Q1.y() * Q2.x() + Q1.z() * Q2.t(); - return GenericQuaternion(x, y, z, t); } From 4a9bf5191c3e7abd6c1194779966c70f27d80810 Mon Sep 17 00:00:00 2001 From: Gvalhca Date: Wed, 26 Jun 2019 01:38:01 +0300 Subject: [PATCH 2/4] Some drone model fixes --- applications/physics/copter/droneObject.cpp | 107 ++++++++++---------- applications/physics/copter/droneObject.h | 2 +- applications/physics/copter/motor.cpp | 32 +++--- applications/physics/copter/motor.h | 14 ++- applications/physics/copter/quad.cpp | 4 +- applications/physics/copter/quad.h | 18 ++-- applications/physics/physMainObject.cpp | 22 +++- applications/physics/physMainObject.h | 1 + applications/physics/physObject.cpp | 19 ++-- applications/physics/physObject.h | 8 +- applications/physics/physSphere.cpp | 5 +- applications/physics/physics.pro | 4 +- applications/physics/physicsMainWindow.cpp | 5 +- applications/physics/physicsMainWindow.h | 4 +- applications/physics/simObject.h | 1 - 15 files changed, 136 insertions(+), 110 deletions(-) diff --git a/applications/physics/copter/droneObject.cpp b/applications/physics/copter/droneObject.cpp index a66abe3ff..859b91eb3 100644 --- a/applications/physics/copter/droneObject.cpp +++ b/applications/physics/copter/droneObject.cpp @@ -5,12 +5,14 @@ using namespace corecvs; -DroneObject::DroneObject(double frameSize, double mass) +DroneObject::DroneObject(double frameSize, double mass) : PhysMainObject() { - PhysMainObject(); setSystemMass(mass); + double propellerRadius = 0.020; /**< propeller radius in m **/ + double motorMass = 0.01; /**< in kg **/ + Affine3DQ defaultPos = Affine3DQ(Vector3dd::Zero()); - motors.resize(4, Motor()); + motors.resize(4, Motor(&defaultPos, &propellerRadius, &motorMass)); motors[MOTOR_FR].name = "FR"; motors[MOTOR_FR].color = RGBColor::Red(); /*Front right*/ motors[MOTOR_BL].name = "BL"; motors[MOTOR_BL].color = RGBColor::Green(); /*Back left*/ motors[MOTOR_FL].name = "FL"; motors[MOTOR_FL].color = RGBColor::Red(); /*Front left*/ @@ -97,12 +99,14 @@ DroneObject::DroneObject(double frameSize, double mass) } } - double massOfCentralSphere = mass - 4 * motors[0].mass; + double massOfCentralSphere = 0.239; //mass - 4 * motors[0].mass; Affine3DQ posOfCentralSphere = Affine3DQ(Vector3dd(0,0,0).normalised()); double radiusOfCentralSphere = arm / 2; centralSphere = PhysSphere(&posOfCentralSphere, &radiusOfCentralSphere, &massOfCentralSphere); //motors.insert(motors.end(), motors.begin(), motors.end()); + L_INFO<< "centralSphere pos: " << centralSphere.getPosVector() + << " , mass: " << centralSphere.mass << " , radius: " << centralSphere.radius; objects.push_back(¢ralSphere); //objects.push_back(&motors[0]); @@ -112,38 +116,6 @@ DroneObject::DroneObject(double frameSize, double mass) } } -void DroneObject::tick(double deltaT) -{ - double radius = centralSphere.radius; - double motorMass = objects[1]->mass; - double centerMass = objects[0]->mass; - double arm = objects[2]->getPosVector().l2Metric(); - double inertialMomentX = 2.0 / 5.0 * centerMass * pow(radius, 2) + 2 * motorMass * pow(arm, 2); - double inertialMomentY = 2.0 / 5.0 * centerMass * pow(radius, 2) + 2 * motorMass * pow(arm, 2); - double inertialMomentZ = 2.0 / 5.0 * centerMass * pow(radius, 2) + 4 * motorMass * pow(arm, 2); - - inertiaTensor = Matrix33(inertialMomentX, 0, 0, - 0, inertialMomentY, 0, - 0, 0, inertialMomentZ); - - - calcForce(); - calcMoment(); - setPosCenter(getPosCenter() + velocity * deltaT); - velocity += (getForce() / getSystemMass()) * deltaT; - - /* We should carefully use inertiaTensor here. It seems like it changes with the frame of reference */ - Vector3dd W = inertiaTensor.inv() * getMomentum(); - Quaternion angularAcceleration = Quaternion::Rotation(W, W.l2Metric()); - - Quaternion q = orientation; - orientation = Quaternion::pow(angularVelocity, deltaT) ^ orientation; - - //orientation.printAxisAndAngle(); - angularVelocity = Quaternion::pow(angularAcceleration, deltaT) ^ angularVelocity; - L_INFO<<"Delta orient: "< 1.0) motors[i].pwm = 1.0; } - //L_INFO<<"Motor True Values: "<mass; + double centerMass = objects[0]->mass; + double arm = objects[2]->getPosVector().l2Metric(); + double inertialMomentX = 2.0 / 5.0 * centerMass * pow(radius, 2) + 2 * motorMass * pow(arm, 2); + double inertialMomentY = 2.0 / 5.0 * centerMass * pow(radius, 2) + 2 * motorMass * pow(arm, 2); + double inertialMomentZ = 2.0 / 5.0 * centerMass * pow(radius, 2) + 4 * motorMass * pow(arm, 2); + + inertiaTensor = Matrix33(inertialMomentX, 0, 0, + 0, inertialMomentY, 0, + 0, 0, inertialMomentZ); + + + calcForce(); + calcMoment(); + setPosCenter(getPosCenter() + velocity * deltaT); + velocity += (getForce() / getSystemMass()) * deltaT; + + /* We should carefully use inertiaTensor here. It seems like it changes with the frame of reference */ + Vector3dd W = inertiaTensor.inv() * getMomentum(); + Quaternion angularAcceleration = Quaternion::Rotation(W, W.l2Metric()); + + Quaternion q = orientation; + orientation = Quaternion::pow(angularVelocity, deltaT) ^ orientation; + + //orientation.printAxisAndAngle(); + angularVelocity = Quaternion::pow(angularAcceleration, deltaT) ^ angularVelocity; + L_INFO<<"Delta orient: "< motors; + std::vector motors; std::vector cameras; - std::vector sensors; - PID pitchPID{0.7, 0.35, 0.35}; - PID rollPID{0.7, 0.35, 0.35}; - PID yawPID{0.7, 0.35, 0.35}; + std::vector sensors; + PIDClass pitchPID{0.7, 0.35, 0.35}; + PIDClass rollPID{0.7, 0.35, 0.35}; + PIDClass yawPID{0.7, 0.35, 0.35}; enum BetaflightMotors { BETAFLIGHT_MOTOR_1 = 0, diff --git a/applications/physics/physMainObject.cpp b/applications/physics/physMainObject.cpp index 0128e4351..0cc01f7b8 100644 --- a/applications/physics/physMainObject.cpp +++ b/applications/physics/physMainObject.cpp @@ -3,7 +3,7 @@ PhysMainObject::PhysMainObject() { - posCenter = Vector3dd(1,1,1); + posCenter = Vector3dd(0.1, 0.1, 0.1); systemMass = 0; force = Vector3dd::Zero(); //moment = Vector3dd::Zero(); @@ -13,10 +13,13 @@ PhysMainObject::PhysMainObject() void PhysMainObject::addForce(const corecvs::Vector3dd &_force) { - for (size_t i = 0; i < objects.size(); ++i) { + /* objects dont need to know about whole system forces + for (size_t i = 0; i < objects.size(); ++i) + { objects[i]->addForce(_force); } - calcForce(); + */ + force += _force; } void PhysMainObject::tick(double deltaT) @@ -78,7 +81,7 @@ void PhysMainObject::calcForce() { f += objects[i]->getForce(); } - setForce(f); + addForce(f); } void PhysMainObject::calcMoment() @@ -102,6 +105,17 @@ const Vector3dd PhysMainObject::getPosCenter() return posCenter; } +void PhysMainObject::startTick() +{ + force = Vector3dd::Zero(); + momentum = Vector3dd::Zero(); + for (size_t i = 0; i < objects.size(); i++) + { + objects[i]->startTick(); + //objects[i]->calcForce(); + } +} + void PhysMainObject::addObject(PhysObject *object) { objects.push_back(object); diff --git a/applications/physics/physMainObject.h b/applications/physics/physMainObject.h index 41b5628dc..589ca0cdd 100644 --- a/applications/physics/physMainObject.h +++ b/applications/physics/physMainObject.h @@ -55,6 +55,7 @@ class PhysMainObject void setPosCenter(const Vector3dd &_pos); const Vector3dd getPosCenter(); + void startTick(); virtual void tick(double deltaT); diff --git a/applications/physics/physObject.cpp b/applications/physics/physObject.cpp index 844ed26eb..6abac1939 100644 --- a/applications/physics/physObject.cpp +++ b/applications/physics/physObject.cpp @@ -29,8 +29,13 @@ void PhysObject::addMoment(const Vector3dd &moment) void PhysObject::calcMoment() { - M = force * getPosVector(); - SYNC_PRINT(("PhysObject::calcMoment() was called")); + M = F * getPosVector(); + SYNC_PRINT(("PhysObject::calcMoment() was called\n")); +} + +void PhysObject::calcForce() +{ + SYNC_PRINT(("PhysObject.calcForce() called and did nothing\n")); } void PhysObject::tick(double deltaT) @@ -60,12 +65,12 @@ Vector3dd PhysObject::getMoment() const void PhysObject::addToMesh(Mesh3D &mesh) { - SYNC_PRINT(("Don't know how to do this")); + SYNC_PRINT(("Don't know how to do this\n")); } void PhysObject::drawMesh(Mesh3D &mesh) { - SYNC_PRINT(("PhysObject can't draw mesh")); + SYNC_PRINT(("PhysObject can't draw mesh\n")); } void PhysObject::setPosition(const Affine3DQ &pos) @@ -89,10 +94,10 @@ PhysObject::PhysObject() L_INFO << "default PhysObject():created"; } -PhysObject::PhysObject(const Affine3DQ *coords, const double *m) +PhysObject::PhysObject(const Affine3DQ &coords, const double &m) { - position = Affine3DQ(*coords); - mass = *m; + position = Affine3DQ(coords); + mass = m; F = Vector3dd::Zero(); M = Vector3dd::Zero(); diff --git a/applications/physics/physObject.h b/applications/physics/physObject.h index dd8ca2bad..0cdaf703b 100644 --- a/applications/physics/physObject.h +++ b/applications/physics/physObject.h @@ -78,9 +78,8 @@ class PhysObject { public: PhysObject(); - PhysObject(const corecvs::Affine3DQ *coords, const double *m); + PhysObject(const corecvs::Affine3DQ &coords, const double &m); - Vector3dd force = corecvs::Vector3dd::Zero(); Vector3dd velocity = Vector3dd::Zero(); int frameCounter = 0; @@ -110,7 +109,7 @@ class PhysObject virtual void saveMesh (const std::string &name); virtual void drawMesh (corecvs::Mesh3D &mesh); virtual void calcMoment(); - + virtual void calcForce(); /* Get & Set */ void setPosition(const corecvs::Affine3DQ &pos); void setPosition(const corecvs::Vector3dd &pos); @@ -127,7 +126,4 @@ class PhysObject }; - - - #endif // PHYSOBJECT_H diff --git a/applications/physics/physSphere.cpp b/applications/physics/physSphere.cpp index a35297c36..76d107968 100644 --- a/applications/physics/physSphere.cpp +++ b/applications/physics/physSphere.cpp @@ -6,6 +6,7 @@ PhysSphere::PhysSphere() { PhysObject(); radius = 1.0; + L_INFO << "Created default PhysSphere"; } void PhysSphere::setPos(const Vector3dd &pos) @@ -13,10 +14,10 @@ void PhysSphere::setPos(const Vector3dd &pos) this->setPosition(Affine3DQ(pos)); } -PhysSphere::PhysSphere(Affine3DQ *pos, double *r, double *m) +PhysSphere::PhysSphere(Affine3DQ *pos, double *r, double *m) : PhysObject(*pos, *m) { - PhysObject(pos, m); radius = *r; + L_INFO << "Created PhysSphere with pos: " << *pos << " , radius: " << *r << " , mass: " << *m; } void PhysSphere::saveMesh(const std::string &name) diff --git a/applications/physics/physics.pro b/applications/physics/physics.pro index e2de22c03..78d773a2a 100644 --- a/applications/physics/physics.pro +++ b/applications/physics/physics.pro @@ -42,7 +42,7 @@ HEADERS += \ joystick/mixerChannelOperationWidget.h \ frameProcessor.h \ protoautopilot.h \ - #copter/quad.h \ + copter/quad.h \ physicsMainWindow.h \ physicsAboutWidget.h \ physObject.h \ @@ -70,7 +70,7 @@ SOURCES += \ joystick/mixerChannelOperationWidget.cpp \ frameProcessor.cpp \ protoautopilot.cpp \ - #copter/quad.cpp \ + copter/quad.cpp \ physicsMainWindow.cpp \ physicsAboutWidget.cpp \ physObject.cpp \ diff --git a/applications/physics/physicsMainWindow.cpp b/applications/physics/physicsMainWindow.cpp index 0b87fad79..c948a7aa5 100644 --- a/applications/physics/physicsMainWindow.cpp +++ b/applications/physics/physicsMainWindow.cpp @@ -569,7 +569,7 @@ void PhysicsMainWindow::mainAction() */ //startJoyStickMode(); - /** +/** copter.flightControllerTick(joystick1.output); copter.physicsTick(); @@ -590,7 +590,8 @@ void PhysicsMainWindow::mainAction() mGraphDialog.addGraphPoint("Z", copter.position.z()); mGraphDialog.update(); - **/ +**/ + drone.flightControllerTick(joystick1.output); drone.physicsTick(); diff --git a/applications/physics/physicsMainWindow.h b/applications/physics/physicsMainWindow.h index c48c1a0de..58d3f6d74 100644 --- a/applications/physics/physicsMainWindow.h +++ b/applications/physics/physicsMainWindow.h @@ -19,7 +19,7 @@ #include #include -//#include +#include #include #include "clientSender.h" @@ -125,7 +125,7 @@ public slots: ControlsMixer mixer; CopterInputs inputs; QTimer copterTimer; - //Quad copter; + Quad copter; DroneObject drone; public slots: diff --git a/applications/physics/simObject.h b/applications/physics/simObject.h index 2bebe18c4..c3ab8be7f 100644 --- a/applications/physics/simObject.h +++ b/applications/physics/simObject.h @@ -74,7 +74,6 @@ class Force { { return force; } - }; class SimObject From 61562f6bd10079302136a62a72b68bd4a1862d57 Mon Sep 17 00:00:00 2001 From: kola1197 Date: Wed, 26 Jun 2019 02:29:06 +0300 Subject: [PATCH 3/4] interface changed --- applications/physics/copterInputsWidget.cpp | 22 ++ applications/physics/copterInputsWidget.h | 4 +- applications/physics/copterInputsWidget.ui | 278 +++++++++--------- .../physics/joystick/joystickInterface.cpp | 1 - 4 files changed, 164 insertions(+), 141 deletions(-) diff --git a/applications/physics/copterInputsWidget.cpp b/applications/physics/copterInputsWidget.cpp index caa517428..161bd8396 100644 --- a/applications/physics/copterInputsWidget.cpp +++ b/applications/physics/copterInputsWidget.cpp @@ -7,6 +7,16 @@ CopterInputsWidget::CopterInputsWidget(QWidget *parent) : { ui->setupUi(this); + labels[0] = ui->throttleLabel; + labels[1] = ui->rollLabel; + labels[2] = ui->pitchLabel; + labels[3] = ui->yawLabel; + + labels[4] = ui->CH5_label; + labels[5] = ui->CH6_label; + labels[6] = ui->CH7_label; + labels[7] = ui->CH8_label; + sliders[0] = ui->Throttle; sliders[1] = ui->Roll; sliders[2] = ui->Pitch; @@ -17,6 +27,16 @@ CopterInputsWidget::CopterInputsWidget(QWidget *parent) : sliders[6] = ui->CH7; sliders[7] = ui->CH8; + channelsNames[0] = "Throttle"; + channelsNames[1] = "Roll"; + channelsNames[2] = "Pitch"; + channelsNames[3] = "Yaw"; + channelsNames[4] = "CH5"; + channelsNames[5] = "CH6"; + channelsNames[6] = "CH7"; + channelsNames[7] = "CH8"; + + uiSliderUpdated(); for (int i = 0; i < CopterInputs::CHANNEL_LAST; i++) { connect(sliders[i], SIGNAL(valueChanged(int)), this, SLOT(uiSliderUpdated())); @@ -35,6 +55,8 @@ void CopterInputsWidget::updateState(CopterInputs inputs) for (int i = 0; i < CopterInputs::CHANNEL_LAST; i++) { sliders[i]->setValue(inputs.axis[i]); + QString q = channelsNames[i] + "-" + "\n" + QString::number(inputs.axis[i]); + labels[i]->setText(q); } this->blockSignals(false); } diff --git a/applications/physics/copterInputsWidget.h b/applications/physics/copterInputsWidget.h index 40aa1724e..fec823e2b 100644 --- a/applications/physics/copterInputsWidget.h +++ b/applications/physics/copterInputsWidget.h @@ -4,6 +4,7 @@ #include "copterInputs.h" #include +#include namespace Ui { class CopterInputsWidget; @@ -19,7 +20,7 @@ class CopterInputsWidget : public QWidget explicit CopterInputsWidget(QWidget *parent = 0); ~CopterInputsWidget(); QSlider *sliders[CopterInputs::CHANNEL_LAST]; - + QLabel *labels[CopterInputs::CHANNEL_LAST]; public slots: void updateState(CopterInputs inputs); @@ -30,6 +31,7 @@ public slots: void uiUpdated(CopterInputs inputs); private: + QString channelsNames[8]; Ui::CopterInputsWidget *ui; }; diff --git a/applications/physics/copterInputsWidget.ui b/applications/physics/copterInputsWidget.ui index f96c45f05..c70cb6398 100644 --- a/applications/physics/copterInputsWidget.ui +++ b/applications/physics/copterInputsWidget.ui @@ -14,53 +14,8 @@ Form - - - - - 0 - 0 - - - - Throttle - - - - - - - Roll - - - - - - - CH5- - - - - - - - - 30 - 0 - - - - - 166 - 0 - - - - - 168 - 16777215 - - + + 900 @@ -78,24 +33,15 @@ - - - - Qt::Horizontal - - - QSizePolicy::Fixed - - - - 78 - 20 - + + + + CH7- - + - - + + 0 @@ -109,31 +55,15 @@ 2100 - 1100 - - - 1100 + 1500 Qt::Vertical - - - - Yaw - - - - - - - - 0 - 0 - - + + 900 @@ -143,19 +73,34 @@ 1500 + + 1500 + - Qt::Vertical + Qt::Horizontal - - - - CH6- + + + + 900 + + + 2100 + + + 1500 + + + 1500 + + + Qt::Horizontal - + 900 @@ -174,6 +119,29 @@ + + + + Yaw + + + + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 78 + 20 + + + + @@ -190,35 +158,42 @@ - - - - CH7- + + + + + 0 + 0 + - - - - - CH8- + Throttle - - - - Qt::Vertical + + + + + 30 + 0 + - + - 20 - 113 + 166 + 0 - - - - + + + 168 + 16777215 + + + + Qt::LeftToRight + 900 @@ -228,16 +203,19 @@ 1500 - - 1500 - Qt::Horizontal - - + + + + + 0 + 0 + + 900 @@ -245,37 +223,25 @@ 2100 - 1500 + 1100 - 1500 + 1100 - Qt::Horizontal + Qt::Vertical - - - - 900 - - - 2100 - - - 1500 - - - 1500 - - - Qt::Horizontal + + + + CH5- - - + + 30 @@ -294,9 +260,6 @@ 16777215 - - Qt::LeftToRight - 900 @@ -306,13 +269,50 @@ 1500 + + 1500 + Qt::Horizontal - - + + + + Qt::Vertical + + + + 20 + 113 + + + + + + + + CH6- + + + + + + + Roll + + + + + + + CH8- + + + + + 0 diff --git a/applications/physics/joystick/joystickInterface.cpp b/applications/physics/joystick/joystickInterface.cpp index 904e1e15f..72014a1f5 100644 --- a/applications/physics/joystick/joystickInterface.cpp +++ b/applications/physics/joystick/joystickInterface.cpp @@ -52,7 +52,6 @@ JoystickConfiguration JoystickInterface::getConfiguration(int joystickDevice) toReturn.buttonNumber = buttons; } - int version = 0; if (ioctl(joystickDevice, JSIOCGVERSION, &version) != -1) { From 737025fcf70e13490f24f6962a6f5c825aa25e14 Mon Sep 17 00:00:00 2001 From: Gvalhca Date: Fri, 28 Jun 2019 18:58:30 +0300 Subject: [PATCH 4/4] Some drone fixes --- applications/physics/copter/droneObject.cpp | 10 +++---- applications/physics/copter/droneObject.h | 2 +- applications/physics/copter/motor.cpp | 8 ++++-- applications/physics/copter/motor.h | 2 +- applications/physics/physMainObject.cpp | 13 ++++++++- applications/physics/physMainObject.h | 4 +-- applications/physics/physObject.cpp | 2 +- applications/physics/physicsMainWindow.cpp | 9 ++++-- applications/physics/physicsMainWindow.h | 2 ++ applications/physics/simulation.cpp | 31 +++++++++++++++------ applications/physics/simulation.h | 12 ++++---- 11 files changed, 64 insertions(+), 31 deletions(-) diff --git a/applications/physics/copter/droneObject.cpp b/applications/physics/copter/droneObject.cpp index 859b91eb3..3ed87f5ad 100644 --- a/applications/physics/copter/droneObject.cpp +++ b/applications/physics/copter/droneObject.cpp @@ -99,7 +99,7 @@ DroneObject::DroneObject(double frameSize, double mass) : PhysMainObject() } } - double massOfCentralSphere = 0.239; //mass - 4 * motors[0].mass; + double massOfCentralSphere = mass - 4 * motors[0].mass; Affine3DQ posOfCentralSphere = Affine3DQ(Vector3dd(0,0,0).normalised()); double radiusOfCentralSphere = arm / 2; centralSphere = PhysSphere(&posOfCentralSphere, &radiusOfCentralSphere, &massOfCentralSphere); @@ -278,7 +278,7 @@ void DroneObject::flightControllerTick(const CopterInputs &input) forceY = yawPID.P * currentError.z() + yawPID.I * deltaT * yawPID.sumOfError + yawPID.D * (currentError.z() - yawPID.prevError) / deltaT; - + L_INFO << "pitch previous error: " << pitchPID.prevError; pitchPID.prevError = currentError.x(); rollPID.prevError = currentError.y(); yawPID.prevError = currentError.z(); @@ -354,13 +354,11 @@ void DroneObject::tick(double deltaT) 0, inertialMomentY, 0, 0, 0, inertialMomentZ); - - calcForce(); - calcMoment(); setPosCenter(getPosCenter() + velocity * deltaT); velocity += (getForce() / getSystemMass()) * deltaT; /* We should carefully use inertiaTensor here. It seems like it changes with the frame of reference */ + L_INFO << "Momentum: " << getMomentum(); Vector3dd W = inertiaTensor.inv() * getMomentum(); Quaternion angularAcceleration = Quaternion::Rotation(W, W.l2Metric()); @@ -369,7 +367,7 @@ void DroneObject::tick(double deltaT) //orientation.printAxisAndAngle(); angularVelocity = Quaternion::pow(angularAcceleration, deltaT) ^ angularVelocity; - L_INFO<<"Delta orient: "<getForce(); + transform = Affine3DQ(orientation, getPosCenter());// * objects[i]->getPosAffine(); + forceToTransform = objects[i]->getForce(); + forceAfterTransform = transform.rotor * objects[i]->getForce(); + f += forceAfterTransform; + L_INFO << "added force: " << forceAfterTransform << "; forceToTransform: " << forceToTransform; } + L_INFO << "final force to add to PhysMainObject: " << f; addForce(f); } diff --git a/applications/physics/physMainObject.h b/applications/physics/physMainObject.h index 589ca0cdd..f872f434b 100644 --- a/applications/physics/physMainObject.h +++ b/applications/physics/physMainObject.h @@ -59,8 +59,8 @@ class PhysMainObject virtual void tick(double deltaT); - void addSphere(Vector3dd coords, double radius); - void addSphere(Vector3dd coords, double radius, corecvs::RGBColor); + //void addSphere(Vector3dd coords, double radius); + //void addSphere(Vector3dd coords, double radius, corecvs::RGBColor); /** * Adds subobject to MainObject and transfers ownership diff --git a/applications/physics/physObject.cpp b/applications/physics/physObject.cpp index 6abac1939..79ca31e2b 100644 --- a/applications/physics/physObject.cpp +++ b/applications/physics/physObject.cpp @@ -29,7 +29,7 @@ void PhysObject::addMoment(const Vector3dd &moment) void PhysObject::calcMoment() { - M = F * getPosVector(); + M = F ^ getPosVector(); SYNC_PRINT(("PhysObject::calcMoment() was called\n")); } diff --git a/applications/physics/physicsMainWindow.cpp b/applications/physics/physicsMainWindow.cpp index c948a7aa5..480d1aa09 100644 --- a/applications/physics/physicsMainWindow.cpp +++ b/applications/physics/physicsMainWindow.cpp @@ -230,7 +230,7 @@ void PhysicsMainWindow::startVirtualMode() mesh->setColor(RGBColor::Red()); for (size_t i = 0; i < simSim.mainObjects.size(); ++i) { - MainObject &mainObj = simSim.mainObjects[i]; + PhysMainObject &mainObj = simSim.mainObjects[i]; for (size_t j = 0; j < mainObj.objects.size(); ++j) { mainObj.objects[j]->addToMesh(*mesh); @@ -275,7 +275,7 @@ void PhysicsMainWindow::keepAlive(){ /* Merge code with :startVirtualMode()*/ for (size_t i = 0; i < simSim.mainObjects.size(); i++) { - MainObject &mainObj = simSim.mainObjects[i]; + PhysMainObject &mainObj = simSim.mainObjects[i]; for (size_t j = 0; j < mainObj.objects.size(); j++) { mainObj.objects[j]->addToMesh(*mesh); @@ -779,3 +779,8 @@ void PhysicsMainWindow::on_toolButton_3_released() { startRealMode(); } + +void PhysicsMainWindow::on_connetToVirtualButton_pressed() +{ + +} diff --git a/applications/physics/physicsMainWindow.h b/applications/physics/physicsMainWindow.h index 58d3f6d74..ce3081e27 100644 --- a/applications/physics/physicsMainWindow.h +++ b/applications/physics/physicsMainWindow.h @@ -188,6 +188,8 @@ private slots: void on_toolButton_3_released(); + void on_connetToVirtualButton_pressed(); + private: struct Message { int throttle; diff --git a/applications/physics/simulation.cpp b/applications/physics/simulation.cpp index fe4e3f29d..eed8c1c07 100644 --- a/applications/physics/simulation.cpp +++ b/applications/physics/simulation.cpp @@ -29,12 +29,22 @@ Simulation::Simulation(string arg) void Simulation::droneStart() { mainObjects.emplace_back(); - MainObject *mainObject = &mainObjects.back(); + PhysMainObject *mainObject = &mainObjects.back(); mainObject->countPhysics = true; - mainObject->addSphere(Vector3dd(-1, -1, -1), 2); - mainObject->addSphere(Vector3dd(1, -1, -1), 2); - mainObject->addSphere(Vector3dd(-1, 1, -1), 2); - mainObject->addSphere(Vector3dd(1, 1, -1), 2); + double radius = 2.0; + double mass = 1.0; + Affine3DQ pos1 = Affine3DQ(Vector3dd(-1, -1, -1)); + Affine3DQ pos2 = Affine3DQ(Vector3dd(1, -1, -1)); + Affine3DQ pos3 = Affine3DQ(Vector3dd(-1, 1, -1)); + Affine3DQ pos4 = Affine3DQ(Vector3dd(1, 1, -1)); + PhysSphere sphere1 = PhysSphere(&pos1, &radius, &mass); + PhysSphere sphere2 = PhysSphere(&pos2, &radius, &mass); + PhysSphere sphere3 = PhysSphere(&pos3, &radius, &mass); + PhysSphere sphere4 = PhysSphere(&pos4, &radius, &mass); + mainObject->addObject(&sphere1); + mainObject->addObject(&sphere2); + mainObject->addObject(&sphere3); + mainObject->addObject(&sphere4); mainObject->addForce(Vector3dd(0,-9.8,0)); @@ -45,9 +55,13 @@ void Simulation::defaultStart() { /* Adds new MainObject to the vector */ mainObjects.emplace_back(); - MainObject *mainObject = &mainObjects.back(); + PhysMainObject *mainObject = &mainObjects.back(); mainObject->countPhysics = true; - mainObject->addSphere(Vector3dd(-1, -1, -1), 2); + double radius = 2.0; + double mass = 1.0; + Affine3DQ pos1 = Affine3DQ(Vector3dd(-1, -1, -1)); + PhysSphere sphere1 = PhysSphere(&pos1, &radius, &mass); + mainObject->addObject(&sphere1); mainObject->addForce(Vector3dd(0,-9.8,0)); cout << "Simulation::Simulation():" << mainObjects[0].objects.size() << " before thread" < - +#include "copter/droneObject.h" class Simulation { public: Simulation(); - vector mainObjects; - + vector mainObjects; + DroneObject drone; std::chrono::high_resolution_clock::time_point oldTime; std::chrono::high_resolution_clock::time_point newTime; std::chrono::high_resolution_clock::time_point startTime;