Js placo (#879)

* moved localization and firmware into folders where they belong. playing with order of folders. added an ros example that makes sense for camera. setting up a ros example that makes sense with walk but the ros layer needs some work

* experimenting with placo

* working on adding placo as the main walk engine

* Fixed someproblems in the webots bridge and urdf/xacro. integrated placo as a walk engine alternative

* added a rough draft of ros layer for placo and it works great out of the box

* removed old walk engine and replaced with placo. reworked the ros layer and removed redundent parameter sharing. General clean up. Got walking loop to work from same code regardless of sim or ros. Fixed parameters and general plumbing. removed sigmanban and other description stuff to make it nicer. Fixed bez2 xacro and some improvements to firmware interface tests. should be ready to go.

.gitignore

@@ -34,3 +34,5 @@ venv/
 .mypy_cache/
 cuda-ubuntu2004.pin.1
 *.webm
+
+.idea/

.pre-commit-config.yaml

@@ -1,6 +1,6 @@
 # Exclude Regex https://www3.ntu.edu.sg/home/ehchua/programming/howto/Regexe.html#zz-1.8
 x-exclude: &exclude
-  exclude: '^(external|cmake-build-debug|.pytest_cache|docs|.idea|.+pb2.py|soccer_description|soccer_firmware|soccerbot/rviz/RoboSoccer.json)'
+  exclude: '^(external|cmake-build-debug|.pytest_cache|docs|.idea|.+pb2.py|soccer_description|soccer_hardware/soccer_firmware|soccerbot/rviz/RoboSoccer.json)'
 
 repos:
   - repo: https://github.com/pre-commit/pre-commit-hooks

.prettierignore

@@ -4,4 +4,4 @@ soccer_webots/protos/postures.json
 .idea
 soccer_strategy/src/communication/robot_state_pb2.py
 soccer_webots/src/soccer_webots/messages_pb2.py
-soccer_firmware/*
+soccer_hardware/soccer_firmware/*

requirements.txt

@@ -6,13 +6,13 @@ roslibpy
 scipy>=1.10.1
 prettytable
 pyserial>=3.5
-transforms3d
+transforms3d~=0.4.1
 construct==2.10.67
 opencv-python>=4.5.5.62
-setuptools
+setuptools~=73.0.1
 shapely
 vispy
-pytest
+pytest~=7.4.0
 protobuf==3.20.2
 keras
 gluoncv
@@ -21,10 +21,10 @@ timeout_decorator
 pytest-timeout
 pytest-rerunfailures
 ruamel.yaml
-filterpy
+filterpy~=1.4.5
 gdown==4.5.4
-pybullet
-pygame
+pybullet~=3.2.5
+pygame~=2.5.0
 ultralytics
 
 # object detection
@@ -39,3 +39,34 @@ psutil==5.9.2; platform_machine == 'x86_64'
 #torch==2.0.1.0+cu117; platform_machine == 'x86_64'
 #torchvision==0.18.1.0+cu117; platform_machine == 'x86_64'
 # TODO why did this install torch
+
+keyring~=18.0.1
+mypy-extensions~=1.0.0
+Cython~=3.0.10
+PyYAML~=6.0.1
+catkin-pkg~=0.5.2
+qpsolvers~=4.3.2
+mujoco~=3.2.0
+torch~=1.13.0+cu117
+psutil~=5.9.8
+placo
+argparse~=1.4.0
+open3d~=0.17.0
+timeout-decorator~=0.5.0
+rospkg~=1.5.1
+python-dateutil~=2.8.2
+empy~=3.3.4
+pymavlink~=2.4.39
+PyQt6~=6.7.0
+python-gnupg~=0.4.5
+pycryptodomex~=3.6.1
+netifaces~=0.10.4
+defusedxml~=0.6.0
+pydot~=1.4.1
+PyGObject~=3.36.0
+PyOpenGL~=3.1.0
+pycairo~=1.16.2
+Twisted~=18.9.0
+autobahn~=17.10.1
+tornado~=6.4
+cbor~=1.0.0

soccer_common/src/soccer_common/__init__.py

@@ -1,10 +1,11 @@
+from soccer_common.pid import PID
 from soccer_common.transformation import Transformation
 from soccer_common.transformation2d import Transformation2D
 
-try:
-    from soccer_common.perception.camera_calculations import CameraCalculations
-    from soccer_common.pid import PID
-except:
-    print("not using camera")
+# try:
+#     from soccer_object_detection.camera import CameraCalculations
+#
+# except:
+#     print("not using camera")
 
 # TODO maybe combine with msgs and descriptions?

soccer_control/soccer_pycontrol/config/bez1/bez1.yaml

@@ -1,48 +1,53 @@
-control_frequency: 0.02
-
-# Path parameters
-walking_torso_height: 0.33 # not sure
-
-torso_step_length: 0.02
-torso_step_length_short_backwards: 0.01
-torso_step_length_short_forwards: 0.025
-
-steps_per_second_default: 2.6
-foot_separation: 0.043
-step_height: 0.02
-
-torso_offset_x_ready: 0.012 # positive is forward
-torso_offset_x: 0.02 # positive is forward
-
-torso_offset_pitch_ready: -0.1
-torso_offset_pitch: 0.2
-
-torso_sidediff_sway: -0.01
-get_ready_rate: 0.03
-#pre_footstep_ratio: 0.05
-post_footstep_ratio: 0.2
-
-prepare_walk_time: 2
-
-walking_Kp: 0.07
-walking_Ki: 0.0000
-walking_Kd: 0.5
-walking_setpoint: 0.27
-walking_offset: -0.06
-
-standing_Kp: 0.07
-standing_Ki: 0
-standing_Kd: 0.5
-standing_setpoint: 0.19
-standing_offset: 0.03
-
-walking_roll_Kp: 0.0
-walking_roll_Ki: 0.05
-walking_roll_Kd: 0.0
-walking_roll_setpoint: 0.0
-
-# 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
+# Path calibration Parameters
+robot_model: bez1
+
+# KINEMATIC DATA
+#: Height of the robot's torso (center between two arms) while walking
+walking_torso_height: 0.315
+arm_0_center: -0.45
+arm_1_center: 2.512
+# merge_fixed_links: true
+
+# STABILIZE
+walking_pitch_kp: 0.0 #2.3  1
+walking_pitch_kd: 0 #1
+walking_pitch_ki: 0.000
+walking_pitch_setpoint: -0.0
+walking_pitch_offset: 0.0
+
+walking_roll_kp: 0.0 #1.5   1
+walking_roll_kd: 0 #0.5
+walking_roll_ki: 0.0
+walking_roll_setpoint: -0.0
+walking_roll_offset: 0.0
+
+### WALK ENGINE
+# Walk parameters - if double_support_ratio is not set to 0, should be greater than replan_frequency
+# Timing parameters
+control_frequency: 0.005
+single_support_duration: 0.25 # Duration of single support phase [s]
+single_support_timesteps: 10 # Number of planning timesteps per single support phase
+double_support_ratio: 0.0 # Ratio of double support (0.0 to 1.0)
+startend_double_support_ratio: 2.0 # Ratio duration of supports for starting and stopping walk
+planned_timesteps: 48 # Number of timesteps planned ahead
+replan_timesteps: 10 # Replanning each n timesteps
+
+# Posture parameters
+walk_com_height: 0.21 # Constant height for the CoM [m]
+walk_foot_height: 0.04 # Height of foot rising while walking [m]
+walk_trunk_pitch: 0.0 # Trunk pitch angle [rad]
+walk_foot_rise_ratio: 0.2 # Time ratio for the foot swing plateau (0.0 to 1.0)
+
+# Feet parameters
+foot_length: 0.1200 # Foot length [m]
+foot_width: 0.072 # Foot width [m]
+feet_spacing: 0.122 # Lateral feet spacing [m]
+zmp_margin: 0.02 # ZMP margin [m]
+foot_zmp_target_x: 0.0 # Reference target ZMP position in the foot [m]
+foot_zmp_target_y: 0.0 # Reference target ZMP position in the foot [m]
+
+# Limit parameters
+walk_max_dtheta: 1 # Maximum dtheta per step [rad]
+walk_max_dy: 0.04 # Maximum dy per step [m]
+walk_max_dx_forward: 0.08 # Maximum dx per step forward [m]
+walk_max_dx_backward: 0.03 # Maximum dx per step backward [m]