added new urdf

compose.yaml

@@ -16,6 +16,7 @@ services:
       service: simulator
 
   soccerbot_no_cuda:
+    image: utrarobosoccer/soccerbot:no_cuda
     network_mode: host
     extends:
       file: tools/docker/compose.autonomy.yaml

soccer_control/soccer_pycontrol/config/assembly/assembly.yaml

@@ -0,0 +1,53 @@
+# Path parameters
+robot_model: bez2
+
+# KINEMATIC DATA
+#: Height of the robot's torso (center between two arms) while walking
+walking_torso_height: 0.37
+arm_0_center: -0.45
+arm_1_center: 2.512
+# merge_fixed_links: true
+
+# STABILIZE
+walking_pitch_kp: 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 #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.01
+single_support_duration: 0.3 # 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.27 # 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.1576 # Foot length [m]
+foot_width: 0.092 # 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]

soccer_control/soccer_pycontrol/config/assembly/assembly_sim.yaml

@@ -0,0 +1,53 @@
+# Path parameters
+robot_model: bez2
+
+# KINEMATIC DATA
+#: Height of the robot's torso (center between two arms) while walking
+walking_torso_height: 0.37
+arm_0_center: -0.45
+arm_1_center: 2.512
+# merge_fixed_links: true
+
+# STABILIZE
+walking_pitch_kp: 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 #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.3 # 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: 1.5 # 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.27 # 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.1576 # Foot length [m]
+foot_width: 0.092 # 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]

soccer_control/soccer_pycontrol/config/assembly/assembly_sim_pybullet.yaml

@@ -0,0 +1,53 @@
+# Path parameters
+robot_model: bez2
+
+# KINEMATIC DATA
+#: Height of the robot's torso (center between two arms) while walking
+walking_torso_height: 0.43
+arm_0_center: -0.45
+arm_1_center: 2.512
+# merge_fixed_links: true
+
+# STABILIZE
+walking_pitch_kp: 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 #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.3 # 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: 1.5 # 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.23 # 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.1576 # Foot length [m]
+foot_width: 0.092 # Foot width [m]
+feet_spacing: 0.122 # Lateral feet spacing [m]
+zmp_margin: 0.01 # 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.1 # Maximum dx per step forward [m]
+walk_max_dx_backward: 0.03 # Maximum dx per step backward [m]

soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/foot_step_planner.py

@@ -82,7 +82,7 @@ def setup_tasks(self):
         shoulder_roll = 0 * np.pi / 180
         shoulder_pitch = -0.45  # 20 * np.pi / 180
         joints_task = self.solver.add_joints_task()
-        if self.robot_model == "bez2":
+        if self.robot_model == "bez2" or self.robot_model == "assembly":
             joints_task.set_joints(
                 {
                     "left_shoulder_roll": shoulder_roll,

soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/navigator.py

@@ -13,7 +13,7 @@
 
 from soccer_common import PID, Transformation
 
-
+# TODO could make it more modular by passing in pybullet stuff or have it at one layer higher so we can reuse code
 # TODO change to trajectory controller
 class Navigator:
     def __init__(self, world: PybulletWorld, bez: Bez, imu_feedback_enabled: bool = False, record_walking_metrics: bool = True):
@@ -68,8 +68,8 @@ def walk(self, target_goal: Union[Transformation, List], ball_mode: bool = False
         elif isinstance(target_goal, list):  # [d_x: float = 0.0, d_y: float = 0.0, d_theta: float = 0.0, nb_steps: int = 10, t_goal: float = 10]
             self.walk_time(target_goal)
 
-        if self.record_walking_metrics and display_metrics:
-            self.display_walking_metrics(show_targets=isinstance(target_goal, Transformation))
+        # if self.record_walking_metrics and display_metrics:
+        #     self.display_walking_metrics(show_targets=isinstance(target_goal, Transformation))
         self.ready()
 
     def find_new_vel(self, goal_loc: list, curr_loc: list = (0, 0)):
@@ -116,6 +116,9 @@ def walk_pose(self, target_goal: Transformation):
             pose = (
                 self.bez.sensors.get_pose()
             )  # self.foot_step_planner.robot.get_T_world_trunk()  # can use self.foot_step_planner.trajectory.get_p_world_CoM(t)
+
+            # TODO should be broken up and a unit test
+            print(self.foot_step_planner.robot.com_world())
             goal = Transformation()
             goal.rotation_matrix = np.matmul(target_goal.rotation_matrix, scipy.linalg.inv(pose.rotation_matrix))
             goal.position = pose.rotation_matrix.T @ target_goal.position - pose.rotation_matrix.T @ pose.position
@@ -215,8 +218,8 @@ def walk_loop(self, t: float):
         # self.foot_step_planner.robot.set_T_world_fbase(T)
         # self.foot_step_planner.robot.update_kinematics()
 
-        if self.record_walking_metrics:
-            self.update_walking_metrics(t)
+        # if self.record_walking_metrics:
+        #     self.update_walking_metrics(t)
 
         return t
 

soccer_control/soccer_pycontrol/test/test_placo.py

@@ -6,7 +6,7 @@
 
 from soccer_common import Transformation
 
-REAL_TIME = False
+REAL_TIME = True
 
 
 class TestPlaco(unittest.TestCase):
@@ -21,15 +21,15 @@ def test_bez1(self):
             real_time=REAL_TIME,
             rate=200,
         )
-        self.bez = Bez(robot_model="bez1", pose=Transformation())
-        walk = Navigator(self.world, self.bez, imu_feedback_enabled=True)
+        self.bez = Bez(robot_model="assembly", pose=Transformation())
+        walk = Navigator(self.world, self.bez, imu_feedback_enabled=False)
         # walk.ready()
         # self.bez.motor_control.set_motor()
         # walk.wait(50)
-        target_goal = [0.08, 0, 0, 3, 10]
+        target_goal = [0.03, 0, 0, 10, 500]
         # target_goal = Transformation(position=[1, 0, 0], euler=[0, 0, 0])
         print("STARTING WALK")
-        walk.walk(target_goal, display_metrics=True)
+        walk.walk(target_goal, display_metrics=False)
         # walk.wait(1000)
 
     # TODO fix

soccer_control/soccer_pycontrol/test/test_pybullet.py

@@ -8,7 +8,7 @@
 
 from soccer_common import Transformation
 
-REAL_TIME = False
+REAL_TIME = True
 
 
 class TestPybullet(unittest.TestCase):
@@ -19,10 +19,10 @@ def tearDown(self):
 
     def test_imu(self):
         self.world = PybulletWorld(camera_yaw=45, real_time=REAL_TIME, rate=100)
-        # pose = Transformation()
-        pose = Transformation(position=[0, 0, 0.070], euler=[0, -1.57, 0])
-        pose = Transformation(position=[0, 0, 0.070], euler=[0, 1.57, 0])
-        self.bez = Bez(robot_model="bez2", pose=pose)
+        pose = Transformation()
+        # pose = Transformation(position=[0, 0, 0.070], euler=[0, -1.57, 0])
+        # pose = Transformation(position=[0, 0, 0.070], euler=[0, 1.57, 0])
+        self.bez = Bez(robot_model="assembly", pose=pose)
         self.world.wait(100)
         for i in range(100):
             [_, pitch, roll] = self.bez.sensors.get_imu()
@@ -38,8 +38,8 @@ def test_foot_sensor(self):
         # TODO add more and fuix the link location
 
     def test_bez_motor_range(self):
-        self.world = PybulletWorld(path="", camera_yaw=90, real_time=REAL_TIME, rate=1000)
-        self.bez = Bez(robot_model="bez2", fixed_base=True, pose=Transformation())
+        self.world = PybulletWorld(path="", camera_yaw=45, real_time=REAL_TIME, rate=1000)
+        self.bez = Bez(robot_model="assembly", fixed_base=True, pose=Transformation())
         self.world.wait(50)
         angles = np.linspace(-np.pi, np.pi)
         for i in range(self.bez.motor_control.numb_of_motors):
@@ -59,15 +59,18 @@ def test_bez_motor_range(self):
         self.world.wait(100)
 
     def test_bez_motor_range_single(self):
-        self.world = PybulletWorld(path="", camera_yaw=90, real_time=REAL_TIME, rate=500)
+        self.world = PybulletWorld(path="", camera_yaw=45, real_time=REAL_TIME, rate=500)
         self.bez = Bez(robot_model="bez1", fixed_base=True, pose=Transformation())
+        # self.bez = Bez(robot_model="assembly", fixed_base=True, pose=Transformation())
         self.world.wait(50)
         angles = np.linspace(-np.pi, np.pi)
 
         for j in angles:
             x = [0.0] * self.bez.motor_control.numb_of_motors
-            x[self.bez.motor_control.motor_names.index("left_ankle_roll")] = j
-            x[self.bez.motor_control.motor_names.index("right_ankle_roll")] = j
+            t = "elbow"
+            x[self.bez.motor_control.motor_names.index("head_pitch")] = j
+            # x[self.bez.motor_control.motor_names.index("right_"+t)] = j
+            # x[self.bez.motor_control.motor_names.index("left_"+t)] = j
 
             pb.setJointMotorControlArray(
                 bodyIndex=self.bez.model.body,

soccer_control/soccer_pycontrol/test/test_walk_ros.py

@@ -30,7 +30,9 @@ def test_walk_ros_local(self):
         # walker.wait(50)
         # walker.goal_callback(PoseStamped())
         # walker.walk(d_x=0.04, t_goal=10)
-        target_goal = Transformation(position=[1, 0, 0], euler=[0, 0, 0])
+        # target_goal = Transformation(position=[1, 0, 0], euler=[0, 0, 0])
+        # walker.walk(target_goal)
+        target_goal = [0.04, 0, 0, 10, 500]
         walker.walk(target_goal)
 
         walker.wait(100)

soccer_control/soccer_trajectories/doc/README.md

@@ -5,3 +5,67 @@
 | `TrajectoryManagerSim` | Interfaces with trajectory and sends to pybullet.              |
 | `Trajectory`           | Interpolates a CSV trajectory for multiple joints.             |
 | `Pybullet`             | Sets up pybullet simulation for basic usage                    |
+
+localization
+ - IPM
+
+Navigation
+ - global planner A*
+ - Local Planner Pursuit
+ - 
+
+Mapping 
+ - Create map
+ - Add obstacles with a decay
+
+Perception
+ - segmentation
+ - object localization - obj size, IPM
+ - point cloud localization - IPM
+ - Mapp & IOU classification comparision
+ - Segmentation comparison
+
+Strategy
+ - Adding basic structure
+ - porting old to new
+ - Team communication
+ - stratgey
+
+Control
+ - Muj= os.environ["ROS_NAMESPACE"]
+        os.system(
+            f"/bin/bash -c 'source /opt/ros/noetic/setup.bash && rosnode kill {robot_ns}/soccer_strategy {robot_ns}/soccer_pycontrol {robot_ns}/soccer_trajectories'"
+        )
+
+        rospy.init_node("soccer_control")
+
+        bez = BezROS()
+        walker = NavigatorRos(bez)
+        # walker.wait(50)
+        # walker.ready()
+        # bez.motor_control.set_motor()
+        # walker.wait(50)
+        # walker.goal_callback(PoseStamped())
+        # walker.walk(d_x=0.04, t_goal=10)
+        # target_goal = Transformation(position=[1, 0, 0], euler=[0, 0, 0])
+        # walker.walk(target_goal)
+        target_goal = [0.04, 0, 0, 10, 500]
+        walker.walk(target_goal)
+
+        walker.wait(100)oco
+ - Placo tuning
+ - COM balancing mode
+   - Betteer balance way paper
+ - Trajectory metrics
+ - Refactoring
+ - Trajectory generation
+ - Adding in imu feedback
+ - ZMP 
+ - PID balancing walking
+ - MPC
+ - LQR
+
+Infrastructure
+ - Refactor dockerfile
+ - Switch to ROS2
+ -