fix urdf

mayataka · Nov 20, 2021 · 38a54ec · 38a54ec
1 parent b793ec9
commit 38a54ec
Show file tree

Hide file tree

Showing 7 changed files with 69 additions and 68 deletions.
diff --git a/README.md b/README.md
@@ -33,9 +33,17 @@ pip install meshcat
 
 
 ## Running the trajectory optimizer
-In `mini_pupper_trajopt/trajopt` directory, you can run examples of the mini-pupper's gait optimization.
-You can change the gait parameters such as step length, step height, step timinigs, etc.
-It visualizes the solution trajectory and save the trajectory.
+In `mini_pupper_trajopt` directory, you can run the mini-pupper's gait optimization, e.g., via `python3 running.py`.
+Then the log file is generated at `mini_pupper_trajopt/rsc`.
+
+Note: this step is totally independent from ROS and Gazebo.
+
 
 ## Deploying the trajectory to the mini-pupper controller
-TODO
+After generating the log file of the optimized trajecotory, you can install the log file via `catkin_make` in your workspace.
+
+## Summary for the Gazebo simulation
+1. Install `Pinocchio`, `robotoc`, and `meshcat-python`.
+2. Run `python3 running.py` at `mini_pupper_trajopt` directory.
+3. Run `catkin_make` at ROS workspace.
+4. Launch Gazebo simulation as `roslaunch mini_pupper_gazebo mini_pupper.launch`
diff --git a/mini_pupper_controller/config/mini_pupper_controller.yaml b/mini_pupper_controller/config/mini_pupper_controller.yaml
@@ -1,26 +1,6 @@
 mini_pupper_controller:
   type: "mini_pupper_trajopt/MiniPupperController"
   publish_rate: 250
-  joints:
-    - base_lf1
-    - lf1_lf2
-    - lf2_lf3
-    - base_rf1
-    - rf1_rf2
-    - rf2_rf3
-    - base_lb1
-    - lb1_lb2
-    - lb2_lb3
-    - base_rb1
-    - rb1_rb2
-    - rb2_rb3
-  constraints:
-    goal_time: 0.0
-    stopped_velocity_tolerance: 1.0
-
-position_controller:
-  type: "position_controllers/JointTrajectoryController"
-  publish_rate: 250
   joints:
     - base_lf1
     - lf1_lf2

diff --git a/mini_pupper_controller/src/mini_pupper_controller.cpp b/mini_pupper_controller/src/mini_pupper_controller.cpp
@@ -12,7 +12,8 @@ MiniPupperController::MiniPupperController()
   : joint_handlers_(),
     q_cmd_() {
   const std::string path_to_log
-      = ros::package::getPath("mini_pupper_trajopt") + "/rsc/trotting/q.log";
+      // = ros::package::getPath("mini_pupper_trajopt") + "/rsc/trotting/q.log";
+      = ros::package::getPath("mini_pupper_trajopt") + "/rsc/running/q.log";
   std::ifstream log;
   log.open(path_to_log);
   std::string line;

diff --git a/mini_pupper_description/urdf/mini_pupper_description.urdf b/mini_pupper_description/urdf/mini_pupper_description.urdf
@@ -106,8 +106,8 @@
     <limit
       lower="-3.14"
       upper="3.14"
-      effort="0"
-      velocity="0" />
+      effort="3.0"
+      velocity="1.0" />
   </joint>
   <link
     name="lf2">
@@ -164,8 +164,8 @@
     <limit
       lower="-3.14"
       upper="3.14"
-      effort="0"
-      velocity="0" />
+      effort="3.0"
+      velocity="1.0" />
   </joint>
   <link
     name="lf3">
@@ -222,8 +222,8 @@
     <limit
       lower="-3.14"
       upper="3.14"
-      effort="0"
-      velocity="0" />
+      effort="3.0"
+      velocity="1.0" />
   </joint>
   <link
     name="lffoot">
@@ -333,8 +333,8 @@
     <limit
       lower="-3.14"
       upper="3.14"
-      effort="0"
-      velocity="0" />
+      effort="3.0"
+      velocity="1.0" />
   </joint>
   <link
     name="rf2">
@@ -391,8 +391,8 @@
     <limit
       lower="-3.14"
       upper="3.14"
-      effort="0"
-      velocity="0" />
+      effort="3.0"
+      velocity="1.0" />
   </joint>
   <link
     name="rf3">
@@ -449,8 +449,8 @@
     <limit
       lower="-3.14"
       upper="3.14"
-      effort="0"
-      velocity="0" />
+      effort="3.0"
+      velocity="1.0" />
   </joint>
   <link
     name="rffoot">
@@ -560,8 +560,8 @@
     <limit
       lower="-3.14"
       upper="3.14"
-      effort="0"
-      velocity="0" />
+      effort="3.0"
+      velocity="1.0" />
   </joint>
   <link
     name="lb2">
@@ -618,8 +618,8 @@
     <limit
       lower="-3.14"
       upper="3.14"
-      effort="0"
-      velocity="0" />
+      effort="3.0"
+      velocity="1.0" />
   </joint>
   <link
     name="lb3">
@@ -676,8 +676,8 @@
     <limit
       lower="-3.14"
       upper="3.14"
-      effort="0"
-      velocity="0" />
+      effort="3.0"
+      velocity="1.0" />
   </joint>
   <link
     name="lbfoot">
@@ -787,8 +787,8 @@
     <limit
       lower="-3.14"
       upper="3.14"
-      effort="0"
-      velocity="0" />
+      effort="3.0"
+      velocity="1.0" />
   </joint>
   <link
     name="rb2">
@@ -845,8 +845,8 @@
     <limit
       lower="-3.14"
       upper="3.14"
-      effort="0"
-      velocity="0" />
+      effort="3.0"
+      velocity="1.0" />
   </joint>
   <link
     name="rb3">
@@ -903,8 +903,8 @@
     <limit
       lower="-3.14"
       upper="3.14"
-      effort="0"
-      velocity="0" />
+      effort="3.0"
+      velocity="1.0" />
   </joint>
   <link
     name="rbfoot">

diff --git a/mini_pupper_gazebo/config/mini_pupper_gazebo_controller.yaml b/mini_pupper_gazebo/config/mini_pupper_gazebo_controller.yaml
@@ -4,15 +4,15 @@ joint_state_controller:
 
 gazebo_ros_control:
   pid_gains:
-    base_lf1:  { p: 100.0, d: 0.0, i: 0.0 }
-    lf1_lf2:   { p: 100.0, d: 0.0, i: 0.0 }
-    lf2_lf3:   { p: 100.0, d: 0.0, i: 0.0 }
-    base_rf1:  { p: 100.0, d: 0.0, i: 0.0 }
-    rf1_rf2:   { p: 100.0, d: 0.0, i: 0.0 }
-    rf2_rf3:   { p: 100.0, d: 0.0, i: 0.0 }
-    base_lb1:  { p: 100.0, d: 0.0, i: 0.0 }
-    lb1_lb2:   { p: 100.0, d: 0.0, i: 0.0 }
-    lb2_lb3:   { p: 100.0, d: 0.0, i: 0.0 }
-    base_rb1:  { p: 100.0, d: 0.0, i: 0.0 }
-    rb1_rb2:   { p: 100.0, d: 0.0, i: 0.0 }
-    rb2_rb3:   { p: 100.0, d: 0.0, i: 0.0 }
+    base_lf1:  { p: 1.0, d: 0.0, i: 0.0 }
+    lf1_lf2:   { p: 1.0, d: 0.0, i: 0.0 }
+    lf2_lf3:   { p: 1.0, d: 0.0, i: 0.0 }
+    base_rf1:  { p: 1.0, d: 0.0, i: 0.0 }
+    rf1_rf2:   { p: 1.0, d: 0.0, i: 0.0 }
+    rf2_rf3:   { p: 1.0, d: 0.0, i: 0.0 }
+    base_lb1:  { p: 1.0, d: 0.0, i: 0.0 }
+    lb1_lb2:   { p: 1.0, d: 0.0, i: 0.0 }
+    lb2_lb3:   { p: 1.0, d: 0.0, i: 0.0 }
+    base_rb1:  { p: 1.0, d: 0.0, i: 0.0 }
+    rb1_rb2:   { p: 1.0, d: 0.0, i: 0.0 }
+    rb2_rb3:   { p: 1.0, d: 0.0, i: 0.0 }
diff --git a/mini_pupper_gazebo/launch/mini_pupper.launch b/mini_pupper_gazebo/launch/mini_pupper.launch
@@ -1,11 +1,11 @@
 <launch>
   <include
     file="$(find gazebo_ros)/launch/empty_world.launch" />
-  <node
+  <!-- <node
     name="tf_footprint_base"
     pkg="tf"
     type="static_transform_publisher"
-    args="0 0 0.5 0 0 0 base_link base_footprint 40" />
+    args="0 0 0.5 0 0 0 base_link base_footprint 40" /> -->
 
   <param name="robot_description"
     command="$(find xacro)/xacro --inorder '$(find mini_pupper_description)/urdf/mini_pupper_description.urdf'"
@@ -14,14 +14,26 @@
     name="spawn_model"
     pkg="gazebo_ros"
     type="spawn_model"
-    args="-urdf -model mini_pupper -param robot_description -x 0.0 -y 0.0 -z 0.064"
+    args="-urdf -model mini_pupper -param robot_description 
+          -x 0.0 -y 0.0 -z 0.064
+          -J base_lf1 0.0
+          -J lf1_lf2 0.8 
+          -J lf2_lf3 -1.4
+          -J base_rf1 0.0
+          -J lf1_rf2 0.8 
+          -J lf2_rf3 -1.4
+          -J base_lb1 0.0
+          -J lf1_lb2 0.8 
+          -J lf2_lb3 -1.4
+          -J base_rb1 0.0
+          -J lf1_rb2 0.8 
+          -J lf2_rb3 -1.4"
     output="screen" />
-  <node
+  <!-- <node
     name="fake_joint_calibration"
     pkg="rostopic"
     type="rostopic"
-    args="pub /calibrated std_msgs/Bool true" />
-
+    args="pub /calibrated std_msgs/Bool true" /> -->
 
   <group ns="/mini_pupper">
     <rosparam file="$(find mini_pupper_gazebo)/config/mini_pupper_gazebo_controller.yaml" command="load"/>

diff --git a/mini_pupper_trajopt/mini_pupper_trajopt/running.py b/mini_pupper_trajopt/mini_pupper_trajopt/running.py
@@ -13,7 +13,7 @@ def __init__(self, path_to_urdf=config.PATH_TO_URDF):
         self.hip_swing_time   = 0.13
         self.flying_time      = 0.07
         self.running_time     = self.front_swing_time + self.hip_swing_time + self.flying_time
-        self.dt = 0.01
+        self.dt = 0.005
         self.t0 = 0.5
         self.cycle = 10
         self.T = self.t0 + self.cycle*self.running_time + 2*self.dt