From 502f4a02a5f093b49937777a0751ecc2d70ffbf4 Mon Sep 17 00:00:00 2001 From: Shuo Date: Thu, 8 Apr 2021 00:19:32 -0400 Subject: [PATCH] fix bugs. add a step by step instruction --- toy_ros_space/src/sim_drone/src/Drone.cpp | 6 +++-- toy_ros_space/src/sim_drone/src/Drone.h | 2 +- toy_ros_space/src/sim_drone/src/Quadrotor.cpp | 7 ++++++ toy_ros_space/src/sim_drone/src/kaka.cpp | 3 +++ toy_ros_space/src/sim_drone/src/test_kaka.cpp | 3 ++- toy_ros_space/src/sim_drone/src/test_quad.cpp | 16 ++++++++------ toy_ros_space/src/sim_drone/step.txt | 22 +++++++++++++++++++ 7 files changed, 48 insertions(+), 11 deletions(-) create mode 100644 toy_ros_space/src/sim_drone/step.txt diff --git a/toy_ros_space/src/sim_drone/src/Drone.cpp b/toy_ros_space/src/sim_drone/src/Drone.cpp index a432886..565793a 100644 --- a/toy_ros_space/src/sim_drone/src/Drone.cpp +++ b/toy_ros_space/src/sim_drone/src/Drone.cpp @@ -239,7 +239,7 @@ void Drone::attitude_ctrl(Eigen::Quaterniond target_attitude, const double des_f w_sq[i] = 0; } } - //ROS_INFO("w0 %4.3f w1 %4.3f, w2 %4.3f, w3 %4.3f", sqrtf(w_sq[0]), sqrtf(w_sq[1]), sqrtf(w_sq[2]),sqrtf(w_sq[3])); + // ROS_INFO("w0 %4.3f w1 %4.3f, w2 %4.3f, w3 %4.3f", sqrtf(w_sq[0]), sqrtf(w_sq[1]), sqrtf(w_sq[2]),sqrtf(w_sq[3])); //ROS_INFO("tor1 %4.3f, tor2 %4.3f, tor3 %4.3f", ctrl_torque(0)/(2*d*kf), ctrl_torque(1)/(2*d*kf), ctrl_torque(2)/(4*km)); quad.set_motor_rpms(sqrtf(w_sq[0]), sqrtf(w_sq[1]), sqrtf(w_sq[2]),sqrtf(w_sq[3])); @@ -254,7 +254,8 @@ Eigen::Quaterniond Drone::ctrl_sub_func1(Eigen::Vector3d tilt_cmd, double target tilt_angle = double_limit(tilt_angle, 0.0, angle_limit/180.0*M_PI); // 2017-8-16 I found that previously I missed this. tilt_angle is limited, but tilt_cmd does not normalize - tilt_cmd = tilt_cmd/tilt_cmd_norm; + if (tilt_cmd_norm>1e-5) + tilt_cmd = tilt_cmd/tilt_cmd_norm; Eigen::Vector3d angle_axis = tilt_cmd.cross(Eigen::Vector3d::UnitZ()); //ROS_INFO("angle: %4.3f|axis: %4.3f %4.3f %4.3f", tilt_angle, angle_axis(0), angle_axis(1), angle_axis(2)); /* get final target attitude */ @@ -305,4 +306,5 @@ void Drone::set_k_p_vert_vel(Eigen::Vector3d setting_vec) void Drone::set_position(Eigen::Vector3d setting_vec) { quad.set_position(setting_vec); + lock_height = setting_vec(2); } diff --git a/toy_ros_space/src/sim_drone/src/Drone.h b/toy_ros_space/src/sim_drone/src/Drone.h index 02e0a90..d2996ca 100644 --- a/toy_ros_space/src/sim_drone/src/Drone.h +++ b/toy_ros_space/src/sim_drone/src/Drone.h @@ -23,7 +23,7 @@ class Drone /* target variables */ /* first we only deal with attitude and height control */ double ctrl_target_height; - double lock_height; + double lock_height = 1.0; double ctrl_target_vertical_z; double ctrl_target_yaw; Eigen::Vector2d ctrl_target_horiz_tilt; diff --git a/toy_ros_space/src/sim_drone/src/Quadrotor.cpp b/toy_ros_space/src/sim_drone/src/Quadrotor.cpp index 1cf7cb8..e6a5e94 100644 --- a/toy_ros_space/src/sim_drone/src/Quadrotor.cpp +++ b/toy_ros_space/src/sim_drone/src/Quadrotor.cpp @@ -1,5 +1,6 @@ #include "Quadrotor.h" #include "utils.h" +#include #include Quadrotor::Quadrotor() @@ -15,6 +16,7 @@ Quadrotor::Quadrotor() type = QUAD_MOTOR_CROSS; /* bug record: does not initialize here */ + motor_rpm = Eigen::Array4d::Zero(); external_force = Eigen::Vector3d::Zero(); external_torque = Eigen::Vector3d::Zero(); } @@ -23,6 +25,7 @@ void Quadrotor::sim_step(double dt) { Eigen::Array4d motor_rpm_dot; Eigen::Array4d motor_rpm_sq = motor_rpm.array().square(); + // std::cout << "motor_rpm_sq" << motor_rpm_sq << std::endl; double thrust = kf*motor_rpm_sq.sum(); Eigen::Vector3d force; @@ -64,9 +67,13 @@ void Quadrotor::sim_step(double dt) torque = torque + external_torque; /* physics simulation */ + // std::cout << "pforce" << force << std::endl; + // std::cout << "ptorque" << torque << std::endl; physics.set_force(force); physics.set_torque(torque); + // std::cout << "physics.get_velocity() " << physics.get_velocity() << std::endl; physics.sim_step(dt); + // std::cout << "physics.get_velocity() " << physics.get_velocity() << std::endl; motor_rpm_dot = (target_motor_rpm - motor_rpm)/motor_time_constant; motor_rpm += motor_rpm_dot*dt; diff --git a/toy_ros_space/src/sim_drone/src/kaka.cpp b/toy_ros_space/src/sim_drone/src/kaka.cpp index c73bdf3..704d5cb 100644 --- a/toy_ros_space/src/sim_drone/src/kaka.cpp +++ b/toy_ros_space/src/sim_drone/src/kaka.cpp @@ -21,6 +21,9 @@ KAKA::KAKA() external_torque = Eigen::Vector3d::Zero(); motor_rpm = Eigen::Array2d::Zero(); // added this in kaka not in quadrotor, + tgt_motor_rpm = Eigen::Array2d::Zero(); + arm_angle = Eigen::Array2d::Zero(); + tgt_arm_angle = Eigen::Array2d::Zero(); // because without it kaka will nan for thrust. // so strange... } diff --git a/toy_ros_space/src/sim_drone/src/test_kaka.cpp b/toy_ros_space/src/sim_drone/src/test_kaka.cpp index 782e741..d562a9b 100644 --- a/toy_ros_space/src/sim_drone/src/test_kaka.cpp +++ b/toy_ros_space/src/sim_drone/src/test_kaka.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include #include @@ -30,7 +31,7 @@ int main ( int argc, char** argv) joint_positions["base_to_right_arm"] = 0.0; urdf::Model model; - model.initFile(std::string("/home/shuo/Desktop/toy_code/toy_ros_space/src/sim_drone/urdf/kaka.urdf")); + model.initFile(ros::package::getPath("sim_drone") + "/urdf/kaka.urdf"); robot_markers::Builder builder(model); visualization_msgs::MarkerArray kaka_vis; builder.Init(); diff --git a/toy_ros_space/src/sim_drone/src/test_quad.cpp b/toy_ros_space/src/sim_drone/src/test_quad.cpp index 85a2c52..27ad6d7 100644 --- a/toy_ros_space/src/sim_drone/src/test_quad.cpp +++ b/toy_ros_space/src/sim_drone/src/test_quad.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include #include "Drone.h" @@ -28,8 +29,8 @@ int main (int argc, char** argv) // because line 98 cannot find right a a = new Drone(0); a->set_position(Eigen::Vector3d(0,-1,1)); - b = new Drone(1); - b->set_position(Eigen::Vector3d(0,1,1)); + // b = new Drone(1); + // b->set_position(Eigen::Vector3d(0,1,1)); ros::Subscriber sub = n.subscribe("/joy", 1000, obtain_joy); setVizMarker(0); //setVizMarker(1); @@ -45,14 +46,14 @@ int main (int argc, char** argv) /* simulation step */ a->sim_step(0.02); - b->sim_step(0.02); + // b->sim_step(0.02); /* update visualization */ Eigen::Quaterniond quaternion = a->get_attitude(); Eigen::Vector3d position = a->get_position(); updateVizMarker(0,position, quaternion); - quaternion = b->get_attitude(); - position = b->get_position(); + // quaternion = b->get_attitude(); + // position = b->get_position(); //updateVizMarker(1,position, quaternion); /* test only, use tf to display rotation */ @@ -81,7 +82,8 @@ void setVizMarker(int id) marker[id].id = id; marker[id].type = visualization_msgs::Marker::MESH_RESOURCE; marker[id].action = visualization_msgs::Marker::ADD; - marker[id].mesh_resource = std::string("package://sim_drone/meshes/hummingbird.mesh"); + marker[id].mesh_resource = "package://sim_drone/meshes/hummingbird.mesh"; + marker[id].scale.x = 0.5; marker[id].scale.y = 0.5; marker[id].scale.z = 0.2; @@ -106,5 +108,5 @@ void updateVizMarker(int id, Eigen::Vector3d _pose, Eigen::Quaterniond _attitude void obtain_joy(const sensor_msgs::Joy::ConstPtr& joy_msg) { a->obtain_joy(joy_msg); - b->obtain_joy(joy_msg); + // b->obtain_joy(joy_msg); } diff --git a/toy_ros_space/src/sim_drone/step.txt b/toy_ros_space/src/sim_drone/step.txt new file mode 100644 index 0000000..b7e46a7 --- /dev/null +++ b/toy_ros_space/src/sim_drone/step.txt @@ -0,0 +1,22 @@ +cd ~ +mkdir tmp_code +cd tmp_code +git clone https://github.com/paulyang1990/toy_code.git +ls +mkdir -p ~/drone_ws/src +cd ~/drone_ws +catkin_make +ls +cd src +ls +cp -r ~/tmp_code/toy_code/toy_ros_space/src/transform_graph . +ls +cd .. +catkin_make +ls +cd src +cp -r ~/tmp_code/toy_code/toy_ros_space/src/sim_drone . +cd .. +catkin_make +source devel/setup.zsh +roslaunch sim_drone test_quad.launch