yeet

soccer_control/soccer_pycontrol/config/bez2/bez2.yaml

@@ -24,11 +24,11 @@ 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
+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: 1.5 # Ratio duration of supports for starting and stopping walk
+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
 

soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/model_ros/bez_ros.py

@@ -12,7 +12,7 @@
 class BezROS(Bez):
     def __init__(self, ns: str = ""):
         self.ns = ns
-        self.robot_model = rospy.get_param("robot_model", "bez1")
+        self.robot_model = rospy.get_param("robot_model", "bez2")
         if rospy.get_param("/use_sim_time", True):
             sim = "_sim"
         else:

soccer_control/soccer_pycontrol/test/test_walk_ros.py

@@ -26,7 +26,7 @@ def test_walk_ros_local(self):
         # bez.motor_control.set_motor()
         # walker.wait(50)
         # walker.goal_callback(PoseStamped())
-        walker.walk(d_x=0.03, t_goal=10)
+        walker.walk(d_x=0.0, t_goal=10)
         # walker.walk(d_x=0.0, d_y=0.06, t_goal=5)
         # walker.walk(d_x=0.0, d_theta=0.3, t_goal=5)
         # walker.walk(d_x=-0.04, t_goal=5)

soccer_hardware/soccer_firmware_interface/config/bez2.yaml

@@ -24,7 +24,7 @@ left_hip_pitch:
 left_knee:
   id: 7
   type: "mx64"
-  angle_zero: 270
+  angle_zero: 180
   angle_max: 360
   angle_min: 0
 
@@ -68,7 +68,7 @@ right_hip_pitch:
 right_knee:
   id: 13
   type: "mx64"
-  angle_zero: 90
+  angle_zero: 180
   angle_max: 360
   angle_min: 0
   flipped: "true"

soccer_hardware/soccer_firmware_interface/test/test_firmware_interface.py

@@ -51,8 +51,8 @@ def test_send_command():
 
 def test_firmware_interface():
     rospy.init_node("firmware_interface")
-    rospy.set_param("motor_mapping", os.path.dirname(os.path.realpath(__file__)) + "/../../config/bez2.yaml")
-    rospy.set_param("motor_types", os.path.dirname(os.path.realpath(__file__)) + "/../../config/motor_types.yaml")
+    rospy.set_param("motor_mapping", os.path.dirname(os.path.realpath(__file__)) + "/../config/bez2.yaml")
+    rospy.set_param("motor_types", os.path.dirname(os.path.realpath(__file__)) + "/../config/motor_types.yaml")
 
     f = FirmwareInterface()
 
@@ -97,13 +97,13 @@ def test_firmware_interface():
 
         time.sleep(0.01)
 
-def test_firmware_interface_single_motor_range(motor_name: str = "head_yaw"):
+def test_firmware_interface_single_motor_range(motor_name: str = "left_knee"):
     rospy.init_node("firmware_interface")
-    rospy.set_param("motor_mapping", os.path.dirname(os.path.realpath(__file__)) + "/../../config/bez2.yaml")
-    rospy.set_param("motor_types", os.path.dirname(os.path.realpath(__file__)) + "/../../config/motor_types.yaml")
+    rospy.set_param("motor_mapping", os.path.dirname(os.path.realpath(__file__)) + "/../config/bez2.yaml")
+    rospy.set_param("motor_types", os.path.dirname(os.path.realpath(__file__)) + "/../config/motor_types.yaml")
 
     f = FirmwareInterface()
-    motor_range = np.linspace(-np.pi, np.pi)
+    motor_range = np.linspace(-np.pi, 0)
     for i in motor_range:
         j = JointState()
         j.name = [
@@ -137,7 +137,7 @@ def test_firmware_interface_single_motor_range(motor_name: str = "head_yaw"):
         j.header.stamp = rospy.Time.now()
 
         f.joint_command_callback(j)
-
+        rospy.sleep(1/10.0)
         time.sleep(0.01)