Skip to content

Commit

Permalink
fix #121
Browse files Browse the repository at this point in the history
  • Loading branch information
@tmori
tmori committed Jan 8, 2024
1 parent 0823f71 commit d7700ab
Show file tree
Hide file tree
Showing 7 changed files with 69 additions and 1 deletion.
1 change: 1 addition & 0 deletions hakoniwa/config/drone_config.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
"airFrictionCoefficient": 0.0001,
"inertia": [ 0.0000625, 0.00003125, 0.00009375 ],
"mass_kg": 0.1,
"body_size": [ 0.1, 0.1, 0.01 ],
"position_meter": [ 0, 0, 0 ],
"angle_degree": [ 0, 0, 90 ]
},
Expand Down
2 changes: 2 additions & 0 deletions hakoniwa/src/assets/drone/aircraft/aircraft_factory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,8 @@ IAirCraft* hako::assets::drone::create_aircraft(const char* drone_type)
HAKO_ASSERT(drone_dynamics != nullptr);
drone_dynamics->set_drag(HAKO_PHYS_DRAG);
drone_dynamics->set_mass(drone_config.getCompDroneDynamicsMass());
auto body_size = drone_config.getCompDroneDynamicsBodySize();
drone_dynamics->set_body_size(body_size[0], body_size[1], body_size[2]);
auto inertia = drone_config.getCompDroneDynamicsInertia();
drone_dynamics->set_torque_constants(inertia[0], inertia[1], inertia[2]);
auto position = drone_config.getCompDroneDynamicsPosition();
Expand Down
1 change: 1 addition & 0 deletions hakoniwa/src/assets/drone/include/idrone_dynamics.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@ class IDroneDynamics: public ICsvLog {
virtual ~IDroneDynamics() {}

virtual void set_drag(double drag) = 0;
virtual void set_body_size(double x, double y, double z) = 0;
virtual void set_torque_constants(double cx, double cy, double cz) = 0;
virtual void set_pos(const DronePositionType &pos) = 0;
virtual void set_vel(const DroneVelocityType &vel) = 0;
Expand Down
35 changes: 34 additions & 1 deletion hakoniwa/src/assets/drone/physics/body_frame/drone_dynamics_body_frame.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,9 @@ class DroneDynamicsBodyFrame : public hako::assets::drone::IDroneDynamics {
double param_cx;
double param_cy;
double param_cz;
double param_size_x;
double param_size_y;
double param_size_z;
/*
* internal state
*/
Expand Down Expand Up @@ -64,8 +67,17 @@ class DroneDynamicsBodyFrame : public hako::assets::drone::IDroneDynamics {
this->param_cx = 1;
this->param_cy = 1;
this->param_cz = 1;
this->param_size_x = 1;
this->param_size_y = 1;
this->param_size_z = 0.1;
}
virtual ~DroneDynamicsBodyFrame() {}
void set_body_size(double x, double y, double z) override
{
this->param_size_x = x;
this->param_size_y = y;
this->param_size_z = z;
}
void set_torque_constants(double cx, double cy, double cz) override
{
this->param_cx = cx;
Expand Down Expand Up @@ -139,7 +151,6 @@ class DroneDynamicsBodyFrame : public hako::assets::drone::IDroneDynamics {
this->angularVelocityBodyFrame,
torque.data.x, torque.data.y, torque.data.z,
this->param_cx, this->param_cy, this->param_cz);

//integral to velocity on body frame
this->velocityBodyFrame.data = integral(this->velocityBodyFrame.data, acc.data);
this->angularVelocityBodyFrame.data = integral(this->angularVelocityBodyFrame.data, acc_angular.data);
Expand All @@ -148,6 +159,28 @@ class DroneDynamicsBodyFrame : public hako::assets::drone::IDroneDynamics {
this->velocity = this->convert(this->velocityBodyFrame);
this->angularVelocity = this->convert(this->angularVelocityBodyFrame);

//collision detection
if (input.collision.collision) {
hako::drone_physics::VectorType velocity_before_contact = this->velocity;
hako::drone_physics::VectorType center_position = this->position;
hako::drone_physics::VectorType contact_position = {
this->position.data.x + input.collision.contact_position[0].x * param_size_x,
this->position.data.y + input.collision.contact_position[0].y * param_size_y,
this->position.data.z + input.collision.contact_position[0].z * param_size_z
};
double restitution_coefficient = input.collision.restitution_coefficient;
//std::cout << "velocity_before_contact.x: " << velocity_before_contact.x << std::endl;
//std::cout << "velocity_before_contact.y: " << velocity_before_contact.y << std::endl;
//std::cout << "velocity_before_contact.z: " << velocity_before_contact.z << std::endl;
hako::drone_physics::VectorType col_vel = hako::drone_physics::velocity_after_contact_with_wall(
velocity_before_contact, center_position, contact_position, restitution_coefficient);
//std::cout << "velocity_after_contact.x: " << col_vel.x << std::endl;
//std::cout << "velocity_after_contact.y: " << col_vel.y << std::endl;
//std::cout << "velocity_after_contact.z: " << col_vel.z << std::endl;
this->velocity = col_vel;
this->velocityBodyFrame = drone_physics::velocity_ground_to_body(this->velocity, angle);
}

//integral to pos, angle on ground frame
this->position.data = integral(this->position.data, this->velocity.data);
this->angle.data = integral(this->angle.data, this->angularVelocity.data);
Expand Down
12 changes: 12 additions & 0 deletions hakoniwa/src/assets/drone/physics/body_frame_rk4/drone_dynamics_body_frame_rk4.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,9 @@ class DroneDynamicsBodyFrameRK4 : public hako::assets::drone::IDroneDynamics {
double param_cx;
double param_cy;
double param_cz;
double param_size_x;
double param_size_y;
double param_size_z;
/*
* internal state
*/
Expand Down Expand Up @@ -85,6 +88,9 @@ class DroneDynamicsBodyFrameRK4 : public hako::assets::drone::IDroneDynamics {
this->param_cx = 1;
this->param_cy = 1;
this->param_cz = 1;
this->param_size_x = 1;
this->param_size_y = 1;
this->param_size_z = 0.1;
}
virtual ~DroneDynamicsBodyFrameRK4() {}
void set_torque_constants(double cx, double cy, double cz) override
Expand All @@ -93,6 +99,12 @@ class DroneDynamicsBodyFrameRK4 : public hako::assets::drone::IDroneDynamics {
this->param_cy = cy;
this->param_cz = cz;
}
void set_body_size(double x, double y, double z) override
{
this->param_size_x = x;
this->param_size_y = y;
this->param_size_z = z;
}
void set_mass(double mass) override
{
this->param_mass = mass;
Expand Down
12 changes: 12 additions & 0 deletions hakoniwa/src/assets/drone/physics/ground_frame/drone_dynamics_ground_frame.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,9 @@ class DroneDynamicsGroundFrame : public hako::assets::drone::IDroneDynamics {
double param_cx;
double param_cy;
double param_cz;
double param_size_x;
double param_size_y;
double param_size_z;
/*
* internal state
*/
Expand Down Expand Up @@ -46,8 +49,17 @@ class DroneDynamicsGroundFrame : public hako::assets::drone::IDroneDynamics {
this->param_cx = 1;
this->param_cy = 1;
this->param_cz = 1;
this->param_size_x = 1;
this->param_size_y = 1;
this->param_size_z = 0.1;
}
virtual ~DroneDynamicsGroundFrame() {}
void set_body_size(double x, double y, double z) override
{
this->param_size_x = x;
this->param_size_y = y;
this->param_size_z = z;
}

void set_torque_constants(double cx, double cy, double cz) override
{
Expand Down
7 changes: 7 additions & 0 deletions hakoniwa/src/config/drone_config.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,6 +119,13 @@ class DroneConfig {
return configJson["components"]["droneDynamics"]["airFrictionCoefficient"].get<double>();
}

std::vector<double> getCompDroneDynamicsBodySize() const {
std::vector<double> body_size;
for (const auto& item : configJson["components"]["droneDynamics"]["body_size"]) {
body_size.push_back(item);
}
return body_size;
}
std::vector<double> getCompDroneDynamicsInertia() const {
std::vector<double> inertia;
for (const auto& item : configJson["components"]["droneDynamics"]["inertia"]) {
Expand Down

0 comments on commit d7700ab

Please sign in to comment.