Bez2 robocup (#845)

* Tested getup on Bez2 hw - fw + fw interface modified to support 2 additional arm motors * fixed getupfront * work in progress getup back joanne * getup/walk WIP * update urdf/xacro with new torques * improved model weights and physics * Get up front updated - joanne * Attempt getup-back by vaulting forward * Getup backflip * IMU fixes + full getup back trajectory * eff * eff * latest stuff on nam's laptop --------- Co-authored-by: nam <[email protected]> Co-authored-by: JoanneT8 <[email protected]> Co-authored-by: manx52 <[email protected]>
utra-robosoccer · Jul 24, 2024 · 8f0dade · 8f0dade
1 parent 24aebbf
commit 8f0dade
Show file tree

Hide file tree

Showing 43 changed files with 464 additions and 205 deletions.
diff --git a/soccer_control/soccer_pycontrol/config/bez2/bez2.yaml b/soccer_control/soccer_pycontrol/config/bez2/bez2.yaml
@@ -1,95 +1,124 @@
-control_frequency: 0.02
-
-# Path parameters
+# Path calibration Parameters
 robot_model: bez2
 
+# KINEMATIC DATA
 #: Height of the robot's torso (center between two arms) while walking
-walking_torso_height: 0.37
+walking_torso_height: 0.375
+arm_0_center: -0.45
+arm_1_center: 2.512
+weird_x_offset: -0.06409
+#: How much the torso is forward in the x axis
+torso_offset_x_ready: 0 # positive is forward
 
-#: How many torso steps per second, approximately equivalent to foot steps per second
-steps_per_second_default: 2.6
+#: How much the torso is rotated in the x axis
+torso_offset_pitch_ready: 0.0
 
-#: How much distance is a torso step (equivalent to a half step)
-torso_step_length: 0.02
-torso_step_length_short_backwards: 0.01
-torso_step_length_short_forwards: 0.025
+#: Dimensions of the foot collision box
+foot_box: [0.09, 0.07, 0.01474]
 
-#: The seperation of the feet while walking
-# How much space between the feet along the path
-foot_separation: 0.047
+#: Transformations from the right foots joint position to the center of the collision box of the foot
+right_foot_joint_center_to_collision_box_center: [0.00385, 0.00401, -0.00737]
+cleats_offset: -0.01634
+
+# HEAD
+head_yaw_freq: 0.02
+head_yaw_freq_relocalizing: 0.005
+head_pitch_freq: 0.02
+head_pitch_freq_relocalizing: 0.005
+head_rotation_yaw_center: 0.175
+head_rotation_yaw_range: 0.15
 
-#: The height of the step from the ground
-step_height: 0.02
+# STEP PLANNER
+#: How much the torso is rotated in the x axis
+torso_offset_pitch: 0.1
 
-#: The distance to the outwards direction when the robot takes a step
-# How much does the foot step outside from the center of the path like a side step
-step_outwardness: 0.01 # positive means away from the Path
+#: How much the torso is forward in the x axis
+torso_offset_x: 0.0 # positive is forward
+
+# STABILIZE
+standing_pitch_kp: 0.
+standing_pitch_kd: 0.0
+standing_pitch_ki: 0.00
+standing_pitch_setpoint: -0.0
+standing_pitch_offset: 0.0
+
+standing_roll_kp: 0.
+standing_roll_kd: 0.0
+standing_roll_ki: 0.00
+standing_roll_setpoint: 0.0
+standing_roll_offset: 0.0
+
+walking_pitch_kp: 0 #1
+walking_pitch_kd: 0
+walking_pitch_ki: 0.000
+walking_pitch_setpoint: -0.01
+walking_pitch_offset: 0.0
+
+walking_roll_kp: 0 #1.5
+walking_roll_kd: 0.0 # 00.5
+walking_roll_ki: 0.0
+walking_roll_setpoint: 0.0
+walking_roll_offset: 0.0
 
-#: The amount of rotation of the footstep
-# Only when it takes a step ?
-step_rotation: 0.05 # positive means the feet turns outwards to the side
+# WALK ENGINE
+control_frequency: 0.01
 
-#: How much the torso is forward in the x axis
-torso_offset_x_ready: -0.06 # positive is forward
-torso_offset_x: 0.02
+# Time before Walk
+prepare_walk_time: 2
 
-#: How much the torso is rotated in the x axis
-torso_offset_pitch_ready: -0.12
-torso_offset_pitch: 0.10
+# PATH
+step_precision: 0.02
 
-# Rate for it to get into the ready state
-get_ready_rate: 0.03
+# PATH FOOT
+# : A half step is taken on the first and the last step to get the robot moving, this parameter indicates the
+# time ratio between the half step and the full step
+half_to_full_step_time_ratio: 0.7
 
 # : Time ratio of a single step in range [0, 1], the ratio of time keeping the feet on the ground before
 # lifting it
-#pre_footstep_ratio: 0.05
+pre_footstep_ratio: 0.15
 
 # : Time ratio of a single step in range  [0, 1], the ratio of time after making the step with the foot on
 # the ground
-#post_footstep_ratio: 0.2
+post_footstep_ratio: 0.25
 
-# Time before Walk
-prepare_walk_time: 1
+#: The seperation of the feet while walking
+# How much space between the feet along the path
+foot_separation: 0.064
 
-# TODO Needs description
-merge_fixed_links: true
+#: The height of the step from the ground
+step_height: 0.065
 
-#: Dimensions of the foot collision box #TODO get it from URDF
-foot_box: [0.09, 0.07, 0.05] # TODO: update this!
+#: The distance to the outwards direction when the robot takes a step
+# How much does the foot step outside from the center of the path like a side step
+step_outwardness: 0.015 # positive means away from the Path
 
-#: Transformations from the right foots joint position to the center of the collision box of the foot
-# (https://docs.google.com/presentation/d/10DKYteySkw8dYXDMqL2Klby-Kq4FlJRnc4XUZyJcKsw/edit#slide=id.g163c1c67b73_0_0)
-right_foot_joint_center_to_collision_box_center: [0.00385, 0.00401, -0.00737] # TODO: update this!
+#: The amount of rotation of the footstep
+# Only when it takes a step ?
+step_rotation: 0.05 # positive means the feet turns outwards to the side
+
+# PATH TORSO
+#: How much the torso bounces up and down while following the torso trajectory (m)
+torso_zdiff_sway: 0
 
 #: How much the torso sways left and right while following the torso trajectory  (m)
-torso_sidediff_sway: -0.01
-
-# TODO Needs description
-walking_Kp: 0
-walking_Ki: 0.0000
-walking_Kd: 0
-walking_setpoint: 0.02
-
-# TODO Needs description
-standing_Kp: 0
-standing_Ki: 0
-standing_Kd: 0.0
-standing_setpoint: 0.0
-
-# TODO Needs description
-walking_roll_Kp: 0.0
-walking_roll_Ki: 0.0
-walking_roll_Kd: 0.0
-walking_roll_setpoint: 0.0
+torso_sidediff_sway:
+  -0.01
 
-# Head rotation parameters
-head_rotation_yaw_center: 0.205
-head_rotation_yaw_range: 0.15
-head_pitch_freq: 0.01
-head_yaw_freq: 0.01
-
-# TODO Needs description
-calibration_trans_a: -0.020357517843533346
-calibration_trans_b: 0.6492919423070727
-calibration_trans_a2: 0.8427201135750022
-calibration_rot_a: 0.7052919512780853
+  #: How much the torso rotates while following the torso trajectory (yaw, pitch, roll)
+torso_thetadiff_sway: [0, 0, 0]
+
+# PATH SECTION
+#: How much distance is a torso step (equivalent to a half step)
+torso_step_length: 0.03
+torso_step_length_short_backwards: 0.025
+torso_step_length_short_forwards: 0.035
+scale_yaw: 1.0 # Increase the rotation by angle
+#: How many torso steps per second, approximately equivalent to foot steps per second
+steps_per_second_default: 2.5
+
+# PATH SECTION BEZIER
+turn_duration: 3
+backwards_torso_step_length_ratio: 0.5
+
+merge_fixed_links: true
diff --git a/soccer_control/soccer_pycontrol/config/bez2/bez2_sim.yaml b/soccer_control/soccer_pycontrol/config/bez2/bez2_sim.yaml
@@ -8,10 +8,10 @@ arm_0_center: -0.45
 arm_1_center: 2.512
 weird_x_offset: -0.06409
 #: How much the torso is forward in the x axis
-torso_offset_x_ready: 0 # positive is forward
+torso_offset_x_ready: 0.012 # positive is forward
 
 #: How much the torso is rotated in the x axis
-torso_offset_pitch_ready: 0.0
+torso_offset_pitch_ready: -0.01
 
 #: Dimensions of the foot collision box
 foot_box: [0.09, 0.07, 0.01474]
@@ -30,10 +30,10 @@ head_rotation_yaw_range: 0.15
 
 # STEP PLANNER
 #: How much the torso is rotated in the x axis
-torso_offset_pitch: 0.05
+torso_offset_pitch: 0.3
 
 #: How much the torso is forward in the x axis
-torso_offset_x: 0.0 # positive is forward
+torso_offset_x: 0.02 # positive is forward
 
 # STABILIZE
 standing_pitch_kp: 0.
@@ -103,19 +103,19 @@ torso_zdiff_sway: 0
 
 #: How much the torso sways left and right while following the torso trajectory  (m)
 torso_sidediff_sway:
-  -0.006
+  0.01
 
   #: How much the torso rotates while following the torso trajectory (yaw, pitch, roll)
 torso_thetadiff_sway: [0, 0, 0]
 
 # PATH SECTION
 #: How much distance is a torso step (equivalent to a half step)
-torso_step_length: 0.06
+torso_step_length: 0.03
 torso_step_length_short_backwards: 0.025
 torso_step_length_short_forwards: 0.035
 scale_yaw: 1.0 # Increase the rotation by angle
 #: How many torso steps per second, approximately equivalent to foot steps per second
-steps_per_second_default: 3
+steps_per_second_default: 1.5
 
 # PATH SECTION BEZIER
 turn_duration: 3

diff --git a/..._control/soccer_pycontrol/src/soccer_pycontrol/model/inverse_kinematics/ik_calculation.py b/..._control/soccer_pycontrol/src/soccer_pycontrol/model/inverse_kinematics/ik_calculation.py
@@ -59,6 +59,10 @@ def right_foot_geometry(self, transformation: Transformation):
         alpha = np.arcsin(num / denom)
         theta5 = np.arctan2(rx, np.sign(rz) * np.sqrt(ry**2 + rz**2)) + alpha
 
+        # offset hack fix
+        # theta4 -= np.arctan(2.4/15)
+        # theta5 += np.arctan(2.4/15) # off by 2mm
+
         return theta4, theta5, theta6
 
     def right_foot_rotation(self, transformation: Transformation, theta4: float, theta5: float, theta6: float):

diff --git a/..._control/soccer_pycontrol/src/soccer_pycontrol/model/inverse_kinematics/kinematic_data.py b/..._control/soccer_pycontrol/src/soccer_pycontrol/model/inverse_kinematics/kinematic_data.py
@@ -87,5 +87,5 @@ def load_urdf(self):
 
 
 if __name__ == "__main__":
-    k = KinematicData(robot_model="bez1")
+    k = KinematicData(robot_model="bez2")
     # k = KinematicData(robot_model="bez2")
diff --git a/...occer_pycontrol/src/soccer_pycontrol/walk_engine/walk_engine_ros/foot_step_planner_ros.py b/...occer_pycontrol/src/soccer_pycontrol/walk_engine/walk_engine_ros/foot_step_planner_ros.py
@@ -12,11 +12,13 @@ def __init__(
         torso_offset_pitch: float = 0.0,
         torso_offset_x: float = 0.0,
     ):
-        robot_model = rospy.get_param("robot_model", "bez1")
+        robot_model = rospy.get_param("robot_model", "bez2")
         if rospy.get_param("/use_sim_time", True):
             sim = "_sim"
         else:
             sim = ""
+
+        sim = "_sim"
         # TODO create path ros
         super(FootStepPlannerROS, self).__init__(
             sim=sim,

diff --git a/...trol/soccer_pycontrol/src/soccer_pycontrol/walk_engine/walk_engine_ros/walk_engine_ros.py b/...trol/soccer_pycontrol/src/soccer_pycontrol/walk_engine/walk_engine_ros/walk_engine_ros.py
@@ -244,20 +244,22 @@ def print_pose(name: str, pose: Pose):
                 self.goal = self.new_goal
                 self.step_planner.robot_path = self.new_path
 
-            # [_, pitch, roll] = self.bez.sensors.get_euler_angles()
-            # path in progress
-            if self.step_planner.robot_path is not None and 0 <= self.t <= self.step_planner.robot_path.duration():
-                # IMU feedback while walking (Average Time: 0.00017305118281667)
-                t_adj = self.t
-                if self.bez.sensors.imu_ready:
-                    # TODO needs to be fixed
-                    pass
-                    # self.stabilize_walk(pitch, roll)
-                    # t_adj = self.soccerbot.apply_phase_difference_roll_feedback(self.t, imu_pose)
-
-                torso_to_right_foot, torso_to_left_foot = self.step_planner.get_next_step(t_adj)
-                self.bez.find_joint_angles(torso_to_right_foot, torso_to_left_foot)
-                self.step_planner.current_step_time = self.t
+            if self.bez.sensors.imu_ready:
+                [_, pitch, roll] = self.bez.sensors.get_euler_angles()
+                # path in progress
+                if self.step_planner.robot_path is not None and 0 <= self.t <= self.step_planner.robot_path.duration():
+
+                    # IMU feedback while walking (Average Time: 0.00017305118281667)
+                    t_adj = self.t
+                    if self.bez.sensors.imu_ready:
+                        # TODO needs to be fixed
+                        pass
+                        # self.stabilize_walk(pitch, roll)
+                        # t_adj = self.soccerbot.apply_phase_difference_roll_feedback(self.t, imu_pose)
+
+                    torso_to_right_foot, torso_to_left_foot = self.step_planner.get_next_step(t_adj)
+                    self.bez.find_joint_angles(torso_to_right_foot, torso_to_left_foot)
+                    self.step_planner.current_step_time = self.t
 
             # Walk completed
             # if (
@@ -290,12 +292,14 @@ def print_pose(name: str, pose: Pose):
             #     if self.step_planner.robot_path is None:
             #         return True
             #
-            #     if self.bez.sensors.imu_ready:
-            #         if self.bez.fallen(pitch):
-            #             return False
+            # print(pitch, roll)
+            if self.bez.sensors.imu_ready:
+                [_, pitch, roll] = self.bez.sensors.get_euler_angles()
+                if self.bez.fallen(pitch):
+                    return False
             # Publishes angles to robot (Average Time: 0.00041992547082119)
             # self.soccerbot.robot_path.show()
-            self.bez.motor_control.set_motor()
+            # self.bez.motor_control.set_motor()
 
             time_end = time.time()
             if time_end - time_start > self.PYBULLET_STEP * 1.2:

diff --git a/soccer_control/soccer_trajectories/src/soccer_trajectories/trajectory_manager_sim.py b/soccer_control/soccer_trajectories/src/soccer_trajectories/trajectory_manager_sim.py
@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+import time
 
 from sensor_msgs.msg import JointState
 from soccer_trajectories.pybullet_setup import PybulletSetup
@@ -51,6 +52,6 @@ def send_trajectory(self, mirror: bool = False) -> None:
             print("time", t)
 
             self.sim.step()
-
+            # time.sleep(0.01)
         # self.sim.ramp.close()
         # self.trajectory.reset()
diff --git a/soccer_control/soccer_trajectories/test/test_trajectory.py b/soccer_control/soccer_trajectories/test/test_trajectory.py
@@ -52,7 +52,7 @@ def test_trajectory(self):
         self.assertEqual(angles, [0.0, 0.0, 0.0, 0.0, 0.564, 0.564, -1.176, -1.176, 0.613, 0.613, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
 
 
-@pytest.mark.parametrize("trajectory_name", ["getupfront"])  # , "getupback", "rightkick", "getupfront"])
+@pytest.mark.parametrize("trajectory_name", ["getupback_roll"])  # , "getupback_full", "rightkick", "getupfront"])
 @pytest.mark.parametrize("robot_model", ["bez2"])
 @pytest.mark.parametrize("real_time", [True])
 def test_trajectory_sim(trajectory_name: str, robot_model: str, real_time: bool):
@@ -64,7 +64,7 @@ def test_trajectory_sim(trajectory_name: str, robot_model: str, real_time: bool)
     if trajectory_name == "getupfront":
         pose = Transformation(position=[0, -0.5, 0.070], euler=[0, 1.57, 1.57])
         camera = 90
-    elif trajectory_name == "getupback":
+    elif trajectory_name == "getupback_full" or trajectory_name == "getupback_roll":
         pose = Transformation(position=[0, 0, 0.070], euler=[0, -1.57, 0])
         camera = 0
     # else:
@@ -106,12 +106,10 @@ def run_real_trajectory(robot_model: str, trajectory_name: str, real_time: bool)
     c.command_callback(command=msg)
     c.send_trajectory(real_time=real_time)
 
-
 # TODO clean up the pybullet interface so that there is a uniform place for the code
 
-
 @pytest.mark.parametrize("robot_model", ["bez2"])
-@pytest.mark.parametrize("trajectory_name", ["getupfront"])
+@pytest.mark.parametrize("trajectory_name", ["getupback_roll"])  # getupback_full
 @pytest.mark.parametrize("real_time", [True])
 # @unittest.skip("Not integrated in CI")
 def test_traj_ros(robot_model: str, trajectory_name: str, real_time: bool):

diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/.~lock.getupback_full.csv# b/soccer_control/soccer_trajectories/trajectories/bez2/.~lock.getupback_full.csv#
@@ -0,0 +1 @@
+,nam,nam-xps15,19.07.2024 04:03,file:///home/nam/.config/libreoffice/4;
diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/.~lock.getupback_left.csv# b/soccer_control/soccer_trajectories/trajectories/bez2/.~lock.getupback_left.csv#
@@ -0,0 +1 @@
+,nam,nam-xps15,19.07.2024 17:46,file:///home/nam/.config/libreoffice/4;
diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/.~lock.getupfront.csv# b/soccer_control/soccer_trajectories/trajectories/bez2/.~lock.getupfront.csv#
@@ -0,0 +1 @@
+,nam,nam-xps15,19.07.2024 12:47,file:///home/nam/.config/libreoffice/4;
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1 @@
		,nam,nam-xps15,19.07.2024 04:03,file:///home/nam/.config/libreoffice/4;
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1 @@
		,nam,nam-xps15,19.07.2024 17:46,file:///home/nam/.config/libreoffice/4;
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1 @@
		,nam,nam-xps15,19.07.2024 12:47,file:///home/nam/.config/libreoffice/4;