change jointdata to suspedata

qiayuanl · Mar 7, 2020 · 66756f3 · 66756f3
1 parent 1f1818d
commit 66756f3
Show file tree

Hide file tree

Showing 8 changed files with 26 additions and 30 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -10,7 +10,7 @@ find_package(catkin REQUIRED COMPONENTS
     )
 add_message_files(
     FILES
-    JointData.msg
+    SuspeData.msg
 )
 
 generate_messages(

diff --git a/config/sim_param.yaml b/config/sim_param.yaml
@@ -1,16 +1,9 @@
 ################################SimParams###########################
 dynamics_dt: 0.00001
 control_dt: 0.001
-home_kp_lin: 2500
-home_kd_lin: 300
-home_kp_ang: 250
-home_kd_ang: 60
 use_spring_damper: false
 go_home: false
 
-#floating point value in table should define with decimal point
-home_pos: [0.,0.,.5]
-home_rpy: [0.,0.,0.]
 
 ###############################SuspeParams##########################
 #Note: all params should be defined at frame on suspension/wheel 0

diff --git a/include/Simulation.h b/include/Simulation.h
@@ -14,15 +14,15 @@
 #include <tf/transform_broadcaster.h>
 #include <rosbag/bag.h>
 #include <geometry_msgs/Twist.h>
-#include <rm_suspension/JointData.h>
+#include <rm_suspension/SuspeData.h>
 
 struct VisData {
   Quat<double> tfQuat[9];
   Vec3<double> tfPos[9];
   vector<Vec3<double >> cpPos;
   vector<Vec3<double>> cpForce;
   geometry_msgs::Twist baseMsg;
-  rm_suspension::JointData jointData;
+  rm_suspension::SuspeData suspeData;
 };
 
 /*!
@@ -74,7 +74,7 @@ class Simulation {
   ros::NodeHandle nh_;
   ros::Publisher markerPub_;
   ros::Publisher twistPub_;
-  ros::Publisher jointPub_;
+  ros::Publisher suspePub_;
   visualization_msgs::Marker marker_;
   tf::TransformBroadcaster br_;
 

diff --git a/include/config.h b/include/config.h
@@ -33,7 +33,7 @@ class SimParameters {
 
     nh->param<double>("dynamics_dt", dynamics_dt_, 0.00001);
     nh->param<double>("control_dt", control_dt_, 0.001);
-    nh->param<double>("vis_fps", vis_fps_, 250);
+    nh->param<double>("vis_fps", vis_fps_, 1000);
     nh->param<double>("floor_kp", floor_kp_, 5000);
     nh->param<double>("floor_kd", floor_kd_, 500000);
     nh->param<double>("home_kp_lin", home_kp_lin_, 2500);

diff --git a/msg/JointData.msg b/msg/JointData.msg
diff --git a/msg/SuspeData.msg b/msg/SuspeData.msg
@@ -0,0 +1,4 @@
+float64[4] spring_length
+float64[4] qd_wheel
+float64[4] tau_suspe
+float64[4] cp_force
diff --git a/src/Dynamics/Chassis.cpp b/src/Dynamics/Chassis.cpp
@@ -84,7 +84,7 @@ FloatingBaseModel<T> Chassis<T>::buildModel() {
       model.addGroundContactPoint
           (bodyID,
            Vec3<T>(_params._wheelRadius * sin(angle), 0, _params._wheelRadius * cos(angle)),
-           true);
+           false);
     }
     //TODO add rfid contact point
   }

diff --git a/src/Simulation.cpp b/src/Simulation.cpp
@@ -20,7 +20,7 @@ Simulation::Simulation()
     sleep(1);
   }
   twistPub_ = nh_.advertise<geometry_msgs::Twist>("base_twist", 100);
-  jointPub_ = nh_.advertise<rm_suspension::JointData>("joint_data", 100);
+  suspePub_ = nh_.advertise<rm_suspension::SuspeData>("suspe_data", 100);
 
   // init parameters
   printf("[Simulation] Load parameters...\n");
@@ -224,14 +224,18 @@ void Simulation::record() {
   //////////////////////////record contact force data///////////////////////////////
   int _nTotalGC = simulator_->getTotalNumGC();
   int count = 0;
-  for (size_t i(0); i < _nTotalGC; ++i) {
+  for (int i = 0; i < _nTotalGC; ++i) {
     Vec3<double> f = simulator_->getContactForce(i);
-    vis_data.cpForce.push_back(f);
-    vis_data.cpPos.push_back(simulator_->getModel()._pGC[i]);
-    count++;
+    if (f.norm() > .0001 && f.norm() < 1000.) {
+      vis_data.cpForce.push_back(f);
+      vis_data.cpPos.push_back(simulator_->getModel()._pGC[i]);
+      count++;
+      if (count < 4) {
+        vis_data.suspeData.cp_force[count] = f.norm();
+      }
+    }
   }
-  vis_data.jointData.cp_count = count;
-  ///////////////////////////////record base twist and joint data for plot//////////////////////////
+  ///////////////////////////////record base twist and suspe data for plot//////////////////////////
   vis_data.baseMsg.linear.x = vis_data.tfPos[0].x();
   vis_data.baseMsg.linear.y = vis_data.tfPos[0].y();
   vis_data.baseMsg.linear.z = vis_data.tfPos[0].z();
@@ -240,10 +244,9 @@ void Simulation::record() {
   vis_data.baseMsg.angular.y = rpy.y();
   vis_data.baseMsg.angular.z = rpy.z();
   for (int wheelID = 0; wheelID < 4; ++wheelID) {
-    vis_data.jointData.q_suspe[wheelID] = fakeSuspe_.getSpringLength(wheelID);
-    vis_data.jointData.qd_suspe[wheelID] = simulator_->getState().qd[wheelID * 2];
-    vis_data.jointData.tau_suspe[wheelID] = tau_[wheelID * 2];
-    vis_data.jointData.qd_wheel[wheelID] = simulator_->getState().qd[wheelID * 2 + 1];
+    vis_data.suspeData.spring_length[wheelID] = fakeSuspe_.getSpringLength(wheelID);
+    vis_data.suspeData.tau_suspe[wheelID] = tau_[wheelID * 2];
+    vis_data.suspeData.qd_wheel[wheelID] = simulator_->getState().qd[wheelID * 2 + 1];
   }
 
   visData_.push_back(vis_data);
@@ -310,6 +313,7 @@ void Simulation::sendCP(const vector<VisData>::iterator &iter, double scale) {
   marker.color.a = 1.0; // Don't forget to set the alpha!
   marker.color.g = 1.0;
   marker.scale.x = .01;
+
   marker.pose.orientation.w = 1;
   marker.id = 5; //large enough to avoid cover marker create at set up
   marker.type = visualization_msgs::Marker::LINE_LIST;
@@ -339,7 +343,7 @@ void Simulation::sendCP(const vector<VisData>::iterator &iter, double scale) {
 
 void Simulation::sendMsg(const vector<VisData>::iterator &iter) {
   twistPub_.publish(iter->baseMsg);
-  jointPub_.publish(iter->jointData);
+  suspePub_.publish(iter->suspeData);
 }
 void Simulation::clearCollision() {
   visualization_msgs::Marker marker;