From 54bd0f6b2db396df47007e78ba305009dfbc5ab6 Mon Sep 17 00:00:00 2001 From: manx52 Date: Mon, 26 Aug 2024 21:38:33 -0400 Subject: [PATCH] yeet --- .../soccer_pycontrol/config/bez2/bez2.yaml | 4 ++-- .../soccer_pycontrol/model/model_ros/bez_ros.py | 2 +- .../soccer_pycontrol/test/test_walk_ros.py | 2 +- .../soccer_firmware_interface/config/bez2.yaml | 4 ++-- .../test/test_firmware_interface.py | 14 +++++++------- 5 files changed, 13 insertions(+), 13 deletions(-) diff --git a/soccer_control/soccer_pycontrol/config/bez2/bez2.yaml b/soccer_control/soccer_pycontrol/config/bez2/bez2.yaml index e6e9032e0..85fa05a07 100644 --- a/soccer_control/soccer_pycontrol/config/bez2/bez2.yaml +++ b/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 diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/model_ros/bez_ros.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/model_ros/bez_ros.py index 7d068a6ef..5769a69c3 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/model_ros/bez_ros.py +++ b/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: diff --git a/soccer_control/soccer_pycontrol/test/test_walk_ros.py b/soccer_control/soccer_pycontrol/test/test_walk_ros.py index cfe7202f3..721e07fd7 100644 --- a/soccer_control/soccer_pycontrol/test/test_walk_ros.py +++ b/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) diff --git a/soccer_hardware/soccer_firmware_interface/config/bez2.yaml b/soccer_hardware/soccer_firmware_interface/config/bez2.yaml index 9859525f3..b348670a1 100644 --- a/soccer_hardware/soccer_firmware_interface/config/bez2.yaml +++ b/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" diff --git a/soccer_hardware/soccer_firmware_interface/test/test_firmware_interface.py b/soccer_hardware/soccer_firmware_interface/test/test_firmware_interface.py index 9c0e56c0d..dbcab885c 100644 --- a/soccer_hardware/soccer_firmware_interface/test/test_firmware_interface.py +++ b/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)