diff --git a/applications/physics/copter/droneObject.cpp b/applications/physics/copter/droneObject.cpp new file mode 100644 index 000000000..3ed87f5ad --- /dev/null +++ b/applications/physics/copter/droneObject.cpp @@ -0,0 +1,391 @@ +#include "droneObject.h" +#include "core/utils/log.h" + +#include + +using namespace corecvs; + +DroneObject::DroneObject(double frameSize, double mass) : 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(&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*/ + 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()); + + L_INFO<< "centralSphere pos: " << centralSphere.getPosVector() + << " , mass: " << centralSphere.mass << " , radius: " << centralSphere.radius; + 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::drawMyself(Mesh3D &mesh) +{ + mesh.mulTransform(getTransform()); + mesh.switchColor(); + + drawBody(mesh); + drawMotors(mesh); + drawCameras(mesh); + drawSensors(mesh); + mesh.popTransform(); + drawForces(mesh); +} + +void DroneObject::drawBody(Mesh3D &mesh) +{ + if (bodyMesh != NULL) + { + mesh.setColor(RGBColor::White()); + mesh.add(*bodyMesh); + mesh.popTransform(); + } +} + +void DroneObject::drawMotors(Mesh3D &mesh) +{ + for (size_t i = 0; i < motors.size(); i++) + { + Motor motor = motors[i]; + mesh.mulTransform(corecvs::Matrix44(motors[i].getPosAffine())); + motor.drawMesh(mesh); + SYNC_PRINT(("motor->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 force = motorToWorld.rotor * motors[i].getForce(); + Vector3dd motorPosition = motorToWorld.shift; + Vector3dd startDot = motorPosition; + Vector3dd endDot = motorPosition + force * 1.0; + mesh.addLine(startDot, endDot); + + //L_INFO << "force: " << force << ", position " << motorPosition; + //L_INFO << "Drew force line from: " << startDot << " , to: " << endDot << " ; Force value: " << force; + } +} + + + +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; + L_INFO << "pitch previous error: " << pitchPID.prevError; + 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: "<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); + + 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()); + + Quaternion q = orientation; + orientation = Quaternion::pow(angularVelocity, deltaT) ^ orientation; + + //orientation.printAxisAndAngle(); + angularVelocity = Quaternion::pow(angularAcceleration, deltaT) ^ angularVelocity; + //L_INFO<<"Delta orient: "< +#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() override; +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..a24dd1253 --- /dev/null +++ b/applications/physics/copter/motor.cpp @@ -0,0 +1,74 @@ +#include "motor.h" + +Motor::Motor() : PhysSphere() +{ + cw = false; + L_INFO << "Created default Motor"; +} + + +Motor::Motor(Affine3DQ *pos, double *propellerRadius, double *mass) + : PhysSphere (pos, propellerRadius, mass) +{ + cw = false; + L_INFO << "Created Motor with pos: " << pos << " , propeller radius: " + << propellerRadius << " , motor mass: " << mass; +} + + +void Motor::calcMoment() +{ + Vector3dd m = getForce() ^ getPosVector(); + addMoment(m); + L_INFO << "Added moment: " << m << "; force : " << getForce() << "; pos: " << getPosVector(); + + Vector3dd v = calcMotorMoment(); + addMoment(v); + 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")); +} + +void Motor::calcForce() +{ + Vector3dd _force = Vector3dd(0, 0, maxForce * pwm); + addForce(_force); + L_INFO << "Motor.calcForce() called. Added force: " << _force; +} + +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 + **/ + Motor(); + + /* Configuration */ + bool cw; + double maxForce = 9.8 / 12; /* Each motor is capable of just lifing itself */ + + double motorWidth = 0.011; /**< in m **/ + double motorHeight = 0.004; /**< 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; + virtual void calcForce() override; + 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(Affine3DQ *pos, double *propellerRadius, double *mass); +}; + +#endif // MOTOR_H diff --git a/applications/physics/copter/quad.cpp b/applications/physics/copter/quad.cpp index 76efd4209..054f1d184 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); @@ -126,7 +126,7 @@ void Quad::drawMyself(Mesh3D &mesh) for (size_t i = 0; i < motors.size(); i++) { - Motor &motor = motors[i]; + MotorClass &motor = motors[i]; mesh.mulTransform(corecvs::Matrix44(getMotorTransfrom(i))); motor.drawMyself(mesh); mesh.popTransform(); @@ -187,7 +187,7 @@ Vector3dd Quad::FromQuaternion(Quaternion &Q) return Vector3dd(pitch, roll, yaw); } -PID::PID(double p, double i, double d) +PIDClass::PIDClass(double p, double i, double d) { P=p; I=i; @@ -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" @@ -14,18 +12,18 @@ #include "simObject.h" -class PID +class PIDClass { public: double P,I,D; double prevError = 0.0; double sumOfError = 0.0; - PID(double p, double i , double d); + PIDClass(double p, double i , double d); }; -class Motor +class MotorClass { public: /** @@ -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,19 +81,19 @@ 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 "< 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/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 3314ec7bc..72014a1f5 100644 --- a/applications/physics/joystick/joystickInterface.cpp +++ b/applications/physics/joystick/joystickInterface.cpp @@ -36,7 +36,7 @@ vector JoystickInterface::getDevices(const string &prefix) } -JoystickConfiguration JoystickInterface::getConfiguration(int joystickDevice) +JoystickConfiguration JoystickInterface::getConfiguration(int joystickDevice) { JoystickConfiguration toReturn; @@ -52,7 +52,6 @@ JoystickConfiguration JoystickInterface::getConfiguration(int joystickDevice) toReturn.buttonNumber = buttons; } - int version = 0; if (ioctl(joystickDevice, JSIOCGVERSION, &version) != -1) { 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..cb27ff731 --- /dev/null +++ b/applications/physics/physMainObject.cpp @@ -0,0 +1,156 @@ +#include "physMainObject.h" + + +PhysMainObject::PhysMainObject() +{ + posCenter = Vector3dd(0.1, 0.1, 0.1); + systemMass = 0; + force = Vector3dd::Zero(); + //moment = Vector3dd::Zero(); + + L_INFO << "default PhysMainObject():created"; +} + +void PhysMainObject::addForce(const corecvs::Vector3dd &_force) +{ + /* objects dont need to know about whole system forces + for (size_t i = 0; i < objects.size(); ++i) + { + objects[i]->addForce(_force); + } + */ + force += _force; +} + +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(); + Affine3DQ transform; + L_INFO << "transform: " << transform; + Vector3dd forceToTransform; + Vector3dd forceAfterTransform; + for(size_t i = 0; i < objects.size(); ++i) + { + 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); +} + +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::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); + 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..f872f434b --- /dev/null +++ b/applications/physics/physMainObject.h @@ -0,0 +1,75 @@ +#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(); + + void startTick(); + + 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..79ca31e2b --- /dev/null +++ b/applications/physics/physObject.cpp @@ -0,0 +1,111 @@ +#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 = 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) +{ + +} + +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\n")); +} + +void PhysObject::drawMesh(Mesh3D &mesh) +{ + SYNC_PRINT(("PhysObject can't draw mesh\n")); +} + +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..0cdaf703b --- /dev/null +++ b/applications/physics/physObject.h @@ -0,0 +1,129 @@ +#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; + L_INFO << "Created default PhysSphere"; +} + +void PhysSphere::setPos(const Vector3dd &pos) +{ + this->setPosition(Affine3DQ(pos)); +} + +PhysSphere::PhysSphere(Affine3DQ *pos, double *r, double *m) : PhysObject(*pos, *m) +{ + radius = *r; + L_INFO << "Created PhysSphere with pos: " << *pos << " , radius: " << *r << " , mass: " << *m; +} + +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..78d773a2a 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 \ @@ -43,9 +44,14 @@ HEADERS += \ protoautopilot.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 \ @@ -66,7 +72,11 @@ SOURCES += \ protoautopilot.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..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); @@ -569,6 +569,7 @@ void PhysicsMainWindow::mainAction() */ //startJoyStickMode(); +/** copter.flightControllerTick(joystick1.output); copter.physicsTick(); @@ -589,6 +590,28 @@ void PhysicsMainWindow::mainAction() 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) { ui->cloud->setNewScenePointer(QSharedPointer(scene), CloudViewDialog::CONTROL_ZONE); @@ -756,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 5107ad06c..ce3081e27 100644 --- a/applications/physics/physicsMainWindow.h +++ b/applications/physics/physicsMainWindow.h @@ -20,6 +20,7 @@ #include #include +#include #include "clientSender.h" #include "copterInputsWidget.h" @@ -125,6 +126,7 @@ public slots: CopterInputs inputs; QTimer copterTimer; Quad copter; + DroneObject drone; public slots: /* Let it be here so far */ @@ -186,6 +188,8 @@ private slots: void on_toolButton_3_released(); + void on_connetToVirtualButton_pressed(); + private: struct Message { int throttle; 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: "<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; diff --git a/core/math/quaternion.h b/core/math/quaternion.h index 239c5c7ce..21fa4353e 100644 --- a/core/math/quaternion.h +++ b/core/math/quaternion.h @@ -114,7 +114,6 @@ class GenericQuaternion : public FixedVectorBase, 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); }