Update pose_controller.py

README.md

@@ -1,9 +1,50 @@
 # robot-gym
 RL and classical techniques applied to robotics.
 
-### Quadruped Playground Demo
-[![playgroud](https://img.youtube.com/vi/n8vaPM3yBlY/0.jpg)](https://www.youtube.com/watch?v=n8vaPM3yBlY)
 
-### Quadruped Go-To taks
-[![playgroud](https://img.youtube.com/vi/OVI9DYjW12k/0.jpg)](https://www.youtube.com/watch?v=OVI9DYjW12k)
+
+#requirements python3.7
+
+#Installation
+
+Create a Python 3.7 virtual environment, e.g. using Anaconda
+
+``conda create -n [envname] python=3.7 anaconda``
+
+``conda activate [envname]``
+
+
+#Install from source
+
+Clone this repository and run from the root of the project:
+
+``git clone https://github.com/nicrusso7/robot-gym.git``
+
+``cd robot-gym``
+
+``git checkout k3lso-stable``
+
+``pip install .``
+
+
+
+
+
+Notes for shapely
+
+``conda config --add channels conda-forge ``
+
+``conda install shapely``
+
+
+
+#Notes
+
+``In robot-gym/robot_gym/model/robots/simple_motor.py``
+The ``(motor_commands)`` on line 133 outputs a 60 dimension vector (q, qdot, kp, kd, tau) = (Postion, velocities, PID gains, motor torque) for each motor. 
+
+
+
+
+
 

robot_gym/controllers/mpc/mpc_controller.py

@@ -42,7 +42,7 @@ def _setup_controller(self, robot, desired_speed=(0.0, 0.0), desired_twisting_sp
             desired_speed=desired_speed,
             desired_twisting_speed=desired_twisting_speed,
             desired_height=self._constants.MPC_BODY_HEIGHT,
-            foot_clearance=0.01)
+            foot_clearance=0.02)
 
         st_controller = torque_stance_leg_controller.TorqueStanceLegController(
             robot,

robot_gym/controllers/pose/pose_controller.py

@@ -88,9 +88,9 @@ def get_action(self):
         #                         [t_rear_right[0], t_rear_right[1], t_rear_right[2]],
         #                         [t_rear_left[0], t_rear_left[1], t_rear_left[2]]])
         signal = [
-            front_right_angles[0], front_right_angles[1], front_right_angles[2],
+            -front_right_angles[0], front_right_angles[1], front_right_angles[2],
             front_left_angles[0], front_left_angles[1], -front_left_angles[2],
-            rear_right_angles[0], -rear_right_angles[1], rear_right_angles[2],
+            -rear_right_angles[0], -rear_right_angles[1], rear_right_angles[2],
             rear_left_angles[0], -rear_left_angles[1], -rear_left_angles[2]
         ]
         return signal

robot_gym/model/robots/k3lso/constants.py

@@ -6,8 +6,8 @@
 INIT_ORIENTATION = [0, 0, 0]
 
 DEFAULT_ABDUCTION_ANGLE = 0
-DEFAULT_HIP_ANGLE = 0.67
-DEFAULT_KNEE_ANGLE = -1.25
+DEFAULT_HIP_ANGLE = 0.75
+DEFAULT_KNEE_ANGLE = -1.35
 
 INIT_MOTOR_ANGLES = np.array(
     [

robot_gym/model/robots/k3lso/ctrl_constants.py

@@ -7,11 +7,13 @@
 # -------------------------------------------------
 MPC_BODY_MASS = 190 / 9.8
 # MPC_BODY_INERTIA = (0.020299, 0, 0, 0, 0.102942, 0, 0, 0, 0.116746)
+#MPC_BODY_INERTIA = (0.07335, 0, 0, 0, 0.25068, 0, 0, 0, 0.25447) #//working
+
 MPC_BODY_INERTIA = (0.07335, 0, 0, 0, 0.25068, 0, 0, 0, 0.25447)
-MPC_BODY_HEIGHT = 0.38
+MPC_BODY_HEIGHT = 0.35
 MPC_VELOCITY_MULTIPLIER = 1.0
 
-STANCE_DURATION_SECONDS = [0.3] * 4  # For faster trotting (v > 1.5 ms reduce this to 0.13s).
+STANCE_DURATION_SECONDS = [0.19] * 4  # For faster trotting (v > 1.5 ms reduce this to 0.13s).
 MAX_TIME_SECONDS = 30.
 
 # Standing
@@ -36,15 +38,15 @@
     gait_generator_lib.LegState.SWING,
 )
 
-VX_OFFSET = 0.
-VY_OFFSET = 0.
-WZ_OFFSET = 0.
+VX_OFFSET = -0.042
+VY_OFFSET = 0.005
+WZ_OFFSET = -0.
 
 # -------------------------------------------------
 # Pose Controller
 # -------------------------------------------------
-l = 0.23
-w = 0.075
+l = 0.246
+w = 0.055
 hip = 0.055
 leg = 0.10652
 foot = 0.145

robot_gym/model/robots/simple_motor.py

@@ -138,6 +138,25 @@ def convert_to_torque(self,
         motor_torques = -1 * (kp * (motor_angle - desired_motor_angles)) - kd * (
                 motor_velocity - desired_motor_velocities) + additional_torques
         motor_torques = self._strength_ratios * motor_torques
+                #Debug output (q, qdot, kp, kd, tau) (Postion, velocities, PID gains, motor torque)
+
+        print("q")
+        print(desired_motor_angles)
+        print(" ")
+        print("qdot")
+        print(motor_velocity)
+        print(" ")
+        print("kp")
+        print(kp)
+        print(" ")
+        print(kd)
+        print(" ")
+        print("tau")
+        print(motor_torques)
+        print(" ")
+        print(" ")
+        print(" ")
+
         if self._torque_limits is not None:
             if len(self._torque_limits) != len(motor_torques):
                 raise ValueError(

robot_gym/util/pybullet_data/robots/k3lso.urdf

@@ -88,6 +88,13 @@
     </collision>
   </link>
   <link name="toe_hr_1">
+    <contact>
+      <friction_anchor/>
+      <stiffness value="30000.0"/>
+      <damping value="1000.0"/>
+      <spinning_friction value="0.1"/>
+      <lateral_friction value="1.0"/>
+    </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0.00458667149031633 -0.006500081766649929 0.010312470108147176"/>
       <mass value="0.03759501582149149"/>
@@ -168,6 +175,13 @@
     </collision>
   </link>
   <link name="toe_hr_2">
+    <contact>
+      <friction_anchor/>
+      <stiffness value="30000.0"/>
+      <damping value="1000.0"/>
+      <spinning_friction value="0.1"/>
+      <lateral_friction value="1.0"/>
+    </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0.004586648204496224 0.006500092995548129 0.01031246157189547"/>
       <mass value="0.03759501582149149"/>
@@ -248,6 +262,13 @@
     </collision>
   </link>
   <link name="toe_hr_3">
+    <contact>
+      <friction_anchor/>
+      <stiffness value="30000.0"/>
+      <damping value="1000.0"/>
+      <spinning_friction value="0.1"/>
+      <lateral_friction value="1.0"/>
+    </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0.004586825372773412 -0.006500081766651011 0.010312470108151173"/>
       <mass value="0.03759501582149149"/>
@@ -328,6 +349,13 @@
     </collision>
   </link>
   <link name="toe_hr_4">
+    <contact>
+      <friction_anchor/>
+      <stiffness value="30000.0"/>
+      <damping value="1000.0"/>
+      <spinning_friction value="0.1"/>
+      <lateral_friction value="1.0"/>
+    </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0.00458680208695289 0.006500092995547518 0.010312461571901133"/>
       <mass value="0.03759501582149149"/>
@@ -352,92 +380,120 @@
     <parent link="base_link"/>
     <child link="abduct_fr_1"/>
     <axis xyz="1.0 0.0 -0.0"/>
+    <limit effort="54" velocity="100"/>
+    <joint_properties damping=".0" friction=".0"/>
   </joint>
   <joint name="abduct_fr_to_thigh_fr_j" type="continuous">
     <origin rpy="0 0 0" xyz="0.05792 -0.031937 -0.000366"/>
     <parent link="abduct_fr_1"/>
     <child link="thigh_hr_1"/>
     <axis xyz="-0.0 1.0 -0.0"/>
+    <limit effort="54" velocity="100"/>
+    <joint_properties damping=".0" friction=".0"/>
   </joint>
   <joint name="thigh_fr_to_knee_fr_j" type="continuous">
     <origin rpy="0 0 0" xyz="0.000114 -0.10152 -0.279885"/>
     <parent link="thigh_hr_1"/>
     <child link="shank_hl_1"/>
     <axis xyz="-0.0 1.0 0.0"/>
+    <limit effort="54" velocity="100"/>
+    <joint_properties damping=".0" friction=".0"/>
   </joint>
   <joint name="fr_joint_toe" type="fixed">
     <origin rpy="0 0 0" xyz="0.002252 0.0177 -0.272251"/>
     <parent link="shank_hl_1"/>
     <child link="toe_hr_1"/>
+    <dynamics damping="0.0" friction="0.0"/>
   </joint>
   <joint name="torso_to_abduct_fl_j" type="continuous">
     <origin rpy="0 0 0" xyz="0.1873 0.055 -0.000895"/>
     <parent link="base_link"/>
     <child link="abduct_fl_1"/>
     <axis xyz="-1.0 -0.0 0.0"/>
+    <limit effort="54" velocity="100"/>
+    <joint_properties damping=".0" friction=".0"/>
   </joint>
   <joint name="abduct_fl_to_thigh_fl_j" type="continuous">
     <origin rpy="0 0 0" xyz="0.05792 0.031937 -0.000366"/>
     <parent link="abduct_fl_1"/>
     <child link="thigh_hl_1"/>
     <axis xyz="0.0 1.0 0.0"/>
+    <limit effort="54" velocity="100"/>
+    <joint_properties damping=".0" friction=".0"/>
   </joint>
   <joint name="thigh_fl_to_knee_fl_j" type="continuous">
     <origin rpy="0 0 0" xyz="-0.000914 0.10152 -0.27875"/>
     <parent link="thigh_hl_1"/>
     <child link="shank_hl_2"/>
     <axis xyz="-0.0 -1.0 -0.0"/>
+    <limit effort="54" velocity="100"/>
+    <joint_properties damping=".0" friction=".0"/>
   </joint>
   <joint name="fl_joint_toe" type="fixed">
     <origin rpy="0 0 0" xyz="0.002252 -0.0177 -0.272251"/>
     <parent link="shank_hl_2"/>
     <child link="toe_hr_2"/>
+    <dynamics damping="0.0" friction="0.0"/>
   </joint>
   <joint name="torso_to_abduct_hr_j" type="continuous">
     <origin rpy="0 0 0" xyz="-0.1873 -0.055 -0.000895"/>
     <parent link="base_link"/>
     <child link="abduct_hr_1"/>
     <axis xyz="1.0 0.0 -0.0"/>
+    <limit effort="54" velocity="100"/>
+    <joint_properties damping=".0" friction=".0"/>
   </joint>
   <joint name="abduct_hr_to_thigh_hr_j" type="continuous">
     <origin rpy="0 0 0" xyz="-0.05792 -0.031937 -0.000366"/>
     <parent link="abduct_hr_1"/>
     <child link="thigh_hr_2"/>
     <axis xyz="-0.0 -1.0 0.0"/>
+    <limit effort="54" velocity="100"/>
+    <joint_properties damping=".0" friction=".0"/>
   </joint>
   <joint name="thigh_hr_to_knee_hr_j" type="continuous">
     <origin rpy="0 0 0" xyz="0.000115 -0.10152 -0.279885"/>
     <parent link="thigh_hr_2"/>
     <child link="shank_hl_3"/>
     <axis xyz="-0.0 1.0 0.0"/>
+    <limit effort="54" velocity="100"/>
+    <joint_properties damping=".0" friction=".0"/>
   </joint>
   <joint name="hr_joint_toe" type="fixed">
     <origin rpy="0 0 0" xyz="0.002251 0.0177 -0.272251"/>
     <parent link="shank_hl_3"/>
     <child link="toe_hr_3"/>
+    <dynamics damping="0.0" friction="0.0"/>
   </joint>
   <joint name="torso_to_abduct_hl_j" type="continuous">
     <origin rpy="0 0 0" xyz="-0.1873 0.055 -0.000895"/>
     <parent link="base_link"/>
     <child link="abduct_hl_1"/>
     <axis xyz="-1.0 -0.0 0.0"/>
+    <limit effort="54" velocity="100"/>
+    <joint_properties damping=".0" friction=".0"/>
   </joint>
   <joint name="abduct_hl_to_thigh_hl_j" type="continuous">
     <origin rpy="0 0 0" xyz="-0.05792 0.031937 -0.000366"/>
     <parent link="abduct_hl_1"/>
     <child link="thigh_hl_2"/>
     <axis xyz="0.0 -1.0 -0.0"/>
+    <limit effort="54" velocity="100"/>
+    <joint_properties damping=".0" friction=".0"/>
   </joint>
   <joint name="thigh_hl_to_knee_hl_j" type="continuous">
     <origin rpy="0 0 0" xyz="-0.000913 0.10152 -0.27875"/>
     <parent link="thigh_hl_2"/>
     <child link="shank_hl_4"/>
     <axis xyz="-0.0 -1.0 -0.0"/>
+    <limit effort="54" velocity="100"/>
+    <joint_properties damping=".0" friction=".0"/>
   </joint>
   <joint name="hl_joint_toe" type="fixed">
     <origin rpy="0 0 0" xyz="0.002251 -0.0177 -0.272251"/>
     <parent link="shank_hl_4"/>
     <child link="toe_hr_4"/>
+    <dynamics damping="0.0" friction="0.0"/>
   </joint>
 </robot>