diff --git a/.gitignore b/.gitignore index a0312c542..0a1f4dc47 100644 --- a/.gitignore +++ b/.gitignore @@ -30,3 +30,4 @@ venv/ /soccer_object_localization/images/ /soccer_object_detection/images/* /soccer_strategy/src/soccer_strategy/log.txt +cuda-ubuntu2004.pin.1 diff --git a/.idea/misc.xml b/.idea/misc.xml index 12c88ba50..526f0ae95 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,5 +1,8 @@ + + diff --git a/README.md b/README.md index 9b8b3841d..343d5146f 100644 --- a/README.md +++ b/README.md @@ -27,3 +27,20 @@ https://github.com/utra-robosoccer/soccerbot/wiki #### Getting Started https://github.com/utra-robosoccer/soccerbot/wiki/Onboarding + +#### Testing Motion on Robot + +```bash +roslaunch soccerbot sensors.launch __ns:=robot1 + +sudo apt install -y python3-pip libopenblas-dev + +python3 -m pip install --upgrade pip; python3 -m pip install numpy==’1.26.1’ python3 -m pip install --no-cache $TORCH_INSTALL + + +export TORCH_INSTALL=path/to/torch-2.2.0a0+81ea7a4+nv23.12-cp38-cp38-linux_aarch64.whl +python3 -m pip install --no-cache $TORCH_INSTALL + + +pip3 install numpy torch-2.1.0a0+41361538.nv23.06-cp38-cp38-linux_aarch64.whl +``` diff --git a/bez2_description/config/motor_mapping.yaml b/bez2_description/config/motor_mapping.yaml index ba1ecc3f2..ea76076c7 100644 --- a/bez2_description/config/motor_mapping.yaml +++ b/bez2_description/config/motor_mapping.yaml @@ -1,33 +1,33 @@ #TODO convert all values to radians motor_mapping: right_leg_motor_0: - id: 5 + id: 10 joint_state_id: 1 direction: -1 #leg turns inwards offset: 180 initial_state: 0.0 right_leg_motor_1: - id: 4 + id: 11 direction: 1 #leg goes outwards offset: 180 initial_state: -0.05 right_leg_motor_2: - id: 3 + id: 12 direction: -1 #leg goes forwards offset: 180 initial_state: 0.25 right_leg_motor_3: - id: 2 + id: 13 direction: 1 #calve goes forwards offset: 180 initial_state: -0.4 right_leg_motor_4: - id: 1 + id: 14 direction: 1 #foot points upwards offset: 180.5 initial_state: 0.2 right_leg_motor_5: - id: 0 + id: 15 direction: -1 #foot turns outwards offset: 180 initial_state: 0.05 @@ -47,37 +47,37 @@ motor_mapping: offset: 180 initial_state: 0.25 left_leg_motor_3: - id: 9 + id: 7 direction: -1 # calve goes forwards offset: 180 initial_state: -0.4 left_leg_motor_4: - id: 10 + id: 8 direction: -1 # foot points upwards offset: 180 initial_state: 0.2 left_leg_motor_5: - id: 11 + id: 9 direction: 1 # foot turns outwards offset: 180 initial_state: 0.05 left_arm_motor_0: - id: 12 + id: 2 direction: 1 # arm goes forwards offset: 180 initial_state: 0.2 left_arm_motor_1: - id: 15 + id: 3 direction: 1 # arm goes forwards offset: 180 initial_state: 1.5 right_arm_motor_0: - id: 14 + id: 0 direction: -1 # arm goes forwards offset: 180 initial_state: 0.2 right_arm_motor_1: - id: 13 + id: 1 direction: -1 # arm goes forwards offset: 180 initial_state: 1.5 diff --git a/requirements.txt b/requirements.txt index baef24455..2f4e8b23a 100644 --- a/requirements.txt +++ b/requirements.txt @@ -36,5 +36,5 @@ Pillow==9.3.0 tqdm gitpython psutil==5.9.2; platform_machine == 'x86_64' -torch==1.13.0+cu117; platform_machine == 'x86_64' -torchvision==0.14.0+cu117; platform_machine == 'x86_64' +torch==2.0.1.0+cu117; platform_machine == 'x86_64' +torchvision==0.15.2.0+cu117; platform_machine == 'x86_64' diff --git a/soccer_firmware_interface/config/bez2.yaml b/soccer_firmware_interface/config/bez2.yaml index d33c0812d..50f63713b 100644 --- a/soccer_firmware_interface/config/bez2.yaml +++ b/soccer_firmware_interface/config/bez2.yaml @@ -1,9 +1,10 @@ left_leg_motor_0: id: 4 - type: "xl430" + type: "xm430" angle_zero: 180 angle_max: 360 angle_min: 0 + initial_state: 0.0 left_leg_motor_1: id: 5 type: "xm430" @@ -11,6 +12,7 @@ left_leg_motor_1: angle_max: 360 angle_min: 0 flipped: "true" + initial_state: -0.05 left_leg_motor_2: id: 6 type: "xm430" @@ -18,18 +20,21 @@ left_leg_motor_2: angle_max: 360 angle_min: 0 flipped: "true" + initial_state: 0.25 left_leg_motor_3: id: 7 type: "mx64" angle_zero: 180 angle_max: 360 angle_min: 0 + initial_state: -0.4 left_leg_motor_4: id: 8 type: "mx64" angle_zero: 180 angle_max: 360 angle_min: 0 + initial_state: 0.2 left_leg_motor_5: id: 9 type: "mx64" @@ -37,6 +42,7 @@ left_leg_motor_5: angle_max: 360 angle_min: 0 flipped: "true" + initial_state: 0.0 right_leg_motor_0: id: 10 type: "xm430" @@ -44,18 +50,21 @@ right_leg_motor_0: angle_max: 360 angle_min: 0 flipped: "true" + initial_state: 0.0 right_leg_motor_1: id: 11 type: "xm430" angle_zero: 180 angle_max: 360 angle_min: 0 + initial_state: -0.05 right_leg_motor_2: id: 12 type: "xm430" angle_zero: 180 angle_max: 360 angle_min: 0 + initial_state: 0.25 right_leg_motor_3: id: 13 type: "mx64" @@ -63,12 +72,14 @@ right_leg_motor_3: angle_max: 360 angle_min: 0 flipped: "true" + initial_state: -0.4 right_leg_motor_4: id: 14 type: "mx64" angle_zero: 180 angle_max: 360 angle_min: 0 + initial_state: 0.2 flipped: "true" right_leg_motor_5: id: 15 @@ -76,6 +87,7 @@ right_leg_motor_5: angle_zero: 180 angle_max: 360 angle_min: 0 + initial_state: 0.0 left_arm_motor_0: id: 2 type: "xm430" @@ -83,18 +95,21 @@ left_arm_motor_0: angle_max: 360 angle_min: 0 flipped: "true" + initial_state: 0.2 left_arm_motor_1: id: 3 type: "xm430" angle_zero: 180 angle_max: 360 angle_min: 0 + initial_state: 1.5 right_arm_motor_0: # Checked id: 0 type: "xm430" angle_zero: 180 angle_max: 360 angle_min: 0 + initial_state: 0.2 right_arm_motor_1: id: 1 type: "xm430" @@ -102,6 +117,7 @@ right_arm_motor_1: angle_max: 360 angle_min: 0 flipped: "true" + initial_state: 1.5 head_motor_0: id: 16 type: "ax12" @@ -109,9 +125,11 @@ head_motor_0: angle_max: 300 angle_min: 0 flipped: "true" + initial_state: 0.0 head_motor_1: id: 17 type: "ax12" angle_zero: 150 angle_max: 300 angle_min: 0 + initial_state: 0.0 diff --git a/soccer_hardware/launch/soccer_hardware.launch b/soccer_hardware/launch/soccer_hardware.launch index 9f71a618a..9d838b36f 100644 --- a/soccer_hardware/launch/soccer_hardware.launch +++ b/soccer_hardware/launch/soccer_hardware.launch @@ -1,6 +1,6 @@ - + @@ -25,4 +25,14 @@ + + diff --git a/soccer_object_detection/src/soccer_object_detection/dfsv.py b/soccer_object_detection/src/soccer_object_detection/dfsv.py new file mode 100644 index 000000000..96d39f065 --- /dev/null +++ b/soccer_object_detection/src/soccer_object_detection/dfsv.py @@ -0,0 +1,23 @@ +import cv2 as cv +import numpy as np + +cap = cv.VideoCapture(4) +if not cap.isOpened(): + print("Cannot open camera") + exit() +while True: + # Capture frame-by-frame + ret, frame = cap.read() + # if frame is read correctly ret is True + if not ret: + print("Can't receive frame (stream end?). Exiting ...") + break + # Our operations on the frame come here + gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY) + # Display the resulting frame + cv.imshow("frame", gray) + if cv.waitKey(1) == ord("q"): + break +# When everything done, release the capture +cap.release() +cv.destroyAllWindows() diff --git a/soccer_object_detection/src/soccer_object_detection/test_object_detection.py b/soccer_object_detection/src/soccer_object_detection/test_object_detection.py index d2c2feceb..acd68fcb9 100644 --- a/soccer_object_detection/src/soccer_object_detection/test_object_detection.py +++ b/soccer_object_detection/src/soccer_object_detection/test_object_detection.py @@ -133,21 +133,122 @@ def test_object_detection_node(self): if best_iou < 0.5: rospy.logwarn(f"bounding boxes lower than 0.5 Image= {file_name} Best IOU={best_iou}") - if "DISPLAY" in os.environ: - cv2.rectangle( - img=mat, - pt1=(best_dimensions[0], best_dimensions[1]), - pt2=(best_dimensions[2], best_dimensions[3]), - color=(255, 255, 255), - ) - if bounding_box.obstacle_detected is True: - cv2.circle(mat, (bounding_box.xbase, bounding_box.ybase), 0, (0, 255, 255), 3) + # if "DISPLAY" in os.environ: + # cv2.rectangle( + # img=mat, + # pt1=(best_dimensions[0], best_dimensions[1]), + # pt2=(best_dimensions[2], best_dimensions[3]), + # color=(255, 255, 255), + # ) + # if bounding_box.obstacle_detected is True: + # cv2.circle(mat, (bounding_box.xbase, bounding_box.ybase), 0, (0, 255, 255), 3) if "DISPLAY" in os.environ: cv2.imshow("Image", mat) cv2.waitKey() cv2.destroyAllWindows() + def test_object_detection_node_cam(self): + + rospy.init_node("test") + + Camera.reset_position = MagicMock() + + src_path = os.path.dirname(os.path.realpath(__file__)) + model_path = src_path + "/../../models/half_5.pt" + + n = ObjectDetectionNode(model_path=model_path) + + n.robot_state.status = RobotState.STATUS_READY + n.game_state.gameState = GameState.GAMESTATE_PLAYING + cap = cv2.VideoCapture(4) + if not cap.isOpened(): + print("Cannot open camera") + exit() + cvbridge = CvBridge() + while True: + ret, frame = cap.read() + if not ret: + print("Can't receive frame (stream end?). Exiting ...") + break + img = cv2.resize(frame, dsize=(640, 480)) + + img_msg: Image = cvbridge.cv2_to_imgmsg(img) + + # Mock the detections + n.pub_detection = MagicMock() + n.pub_boundingbox = MagicMock() + n.pub_detection.get_num_connections = MagicMock(return_value=1) + n.pub_boundingbox.get_num_connections = MagicMock(return_value=1) + n.pub_detection.publish = MagicMock() + n.pub_boundingbox.publish = MagicMock() + + ci = CameraInfo() + ci.height = img.shape[0] + ci.width = img.shape[1] + n.camera.camera_info = ci + n.camera.pose.orientation_euler = [0, np.pi / 8, 0] + n.callback(img_msg) + + if "DISPLAY" in os.environ: + mat = cvbridge.imgmsg_to_cv2(n.pub_detection.publish.call_args[0][0]) + cv2.imshow("Image", mat) + cv2.waitKey(1) + # cv2.destroyAllWindows() + + # Check assertion + if n.pub_boundingbox.publish.call_args is not None: + for bounding_box in n.pub_boundingbox.publish.call_args[0][0].bounding_boxes: + if bounding_box.probability >= n.CONFIDENCE_THRESHOLD and int(bounding_box.Class) in [Label.BALL.value, Label.ROBOT.value]: + bounding_boxes = [ + bounding_box.xmin, + bounding_box.ymin, + bounding_box.xmax, + bounding_box.ymax, + ] + + best_iou = 0 + best_dimensions = None + # for line in lines: + # info = line.split(" ") + # label = int(info[0]) + # if label != int(bounding_box.Class): + # continue + # + # x = float(info[1]) + # y = float(info[2]) + # width = float(info[3]) + # height = float(info[4]) + # + # xmin = int((x - width / 2) * ci.width) + # ymin = int((y - height / 2) * ci.height) + # xmax = int((x + width / 2) * ci.width) + # ymax = int((y + height / 2) * ci.height) + # ground_truth_boxes = [xmin, ymin, xmax, ymax] + # iou = IoU(bounding_boxes, ground_truth_boxes) + # if iou > best_iou: + # best_iou = iou + # best_dimensions = ground_truth_boxes + + # self.assertGreater(best_iou, 0.05, f"bounding boxes are off by too much! Image= {file_name} Best IOU={best_iou}") + # if best_iou < 0.5: + # rospy.logwarn(f"bounding boxes lower than 0.5 Image= {file_name} Best IOU={best_iou}") + + # if "DISPLAY" in os.environ: + # cv2.rectangle( + # img=mat, + # pt1=(best_dimensions[0], best_dimensions[1]), + # pt2=(best_dimensions[2], best_dimensions[3]), + # color=(255, 255, 255), + # ) + # if bounding_box.obstacle_detected is True: + # cv2.circle(mat, (bounding_box.xbase, bounding_box.ybase), 0, (0, 255, 255), 3) + + if "DISPLAY" in os.environ: + cv2.imshow("Image", mat) + cv2.waitKey(1) + # cv2.destroyAllWindows() + @pytest.mark.skip(reason="Only run locally") def test_visualize_annotations(self): src_path = os.path.dirname(os.path.realpath(__file__)) diff --git a/soccer_pycontrol/config/bez2.yaml b/soccer_pycontrol/config/bez2.yaml index 8ccf965f6..b96ce9351 100644 --- a/soccer_pycontrol/config/bez2.yaml +++ b/soccer_pycontrol/config/bez2.yaml @@ -1,34 +1,94 @@ +control_frequency: 0.02 + # Path parameters robot_model: bez2 +#: Height of the robot's torso (center between two arms) while walking walking_torso_height: 0.37 -steps_per_second_default: 2.65 -torso_step_length: 0.03 -torso_step_length_short_backwards: 0.03 -torso_step_length_short_forwards: 0.03 -foot_separation: 0.050 -step_outwardness: 0.035 -torso_offset_x_ready: -0.08 # positive is forward -torso_offset_x: 0 -torso_offset_pitch_ready: -0.05 -torso_offset_pitch: 0.12 + +#: How many torso steps per second, approximately equivalent to foot steps per second +steps_per_second_default: 2.6 + +#: 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 + +#: The seperation of the feet while walking +# How much space between the feet along the path +foot_separation: 0.047 + +#: The height of the step from the ground +step_height: 0.02 + +#: 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 + +#: 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 + +#: How much the torso is forward in the x axis +torso_offset_x_ready: -0.06 # positive is forward +torso_offset_x: 0.02 + +#: How much the torso is rotated in the x axis +torso_offset_pitch_ready: -0.12 +torso_offset_pitch: 0.10 + +# Rate for it to get into the ready state +get_ready_rate: 0.03 + +# : 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 + +# : 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 + +# Time before Walk +prepare_walk_time: 1 + +# TODO Needs description merge_fixed_links: true + +#: Dimensions of the foot collision box #TODO get it from URDF foot_box: [0.09, 0.07, 0.05] # TODO: update this! + +#: 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! -torso_sidediff_sway: 0.01 -step_height: 0.04 +#: 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 + +# 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 diff --git a/soccer_trajectories/src/soccer_trajectories/soccer_trajectories.py b/soccer_trajectories/src/soccer_trajectories/soccer_trajectories.py index 427309481..bdde6bf1e 100755 --- a/soccer_trajectories/src/soccer_trajectories/soccer_trajectories.py +++ b/soccer_trajectories/src/soccer_trajectories/soccer_trajectories.py @@ -32,13 +32,13 @@ def __init__(self, trajectory_path: str, mirror=False): self.time_to_last_pose = 2 # seconds self.trajectory_path = trajectory_path - last_joint_state = JointState() - try: - last_joint_state = rospy.wait_for_message("joint_states", JointState, timeout=2) - except (ROSException, AttributeError) as ex: - rospy.logerr(ex) - except ValueError as ex: - print(ex) + # last_joint_state = JointState() + # try: + # last_joint_state = rospy.wait_for_message("joint_states", JointState, timeout=2) + # except (ROSException, AttributeError) as ex: + # rospy.logerr(ex) + # except ValueError as ex: + # print(ex) with open(trajectory_path) as f: csv_traj = csv.reader(f) @@ -48,16 +48,17 @@ def __init__(self, trajectory_path: str, mirror=False): continue if joint_name == "time": self.times = list(map(float, row[1:])) - self.times = [0] + self.times # + [self.times[-1] + self.time_to_last_pose] + self.times = [0] + self.times + [self.times[-1] + self.time_to_last_pose] self.max_time = self.times[-1] else: joint_values = list(map(float, row[1:])) - last_pose_value = 0 - if joint_name in last_joint_state.name: - last_pose_value = last_joint_state.position[last_joint_state.name.index(joint_name)] + last_pose_value = float(rospy.get_param(f"motor_mapping/{joint_name}/initial_state")) + # last_pose_value = 0.0 + # if joint_name in last_joint_state.name: + # last_pose_value = last_joint_state.position[last_joint_state.name.index(joint_name)] - joint_values = [last_pose_value] + joint_values # + [last_pose_value] + joint_values = [last_pose_value] + joint_values + [last_pose_value] self.splines[joint_name] = interp1d(self.times, joint_values) def get_setpoint(self, timestamp): diff --git a/soccer_trajectories/src/soccer_trajectories/test_trajectory.py b/soccer_trajectories/src/soccer_trajectories/test_trajectory.py index d9b5e0190..d6d65ea0c 100644 --- a/soccer_trajectories/src/soccer_trajectories/test_trajectory.py +++ b/soccer_trajectories/src/soccer_trajectories/test_trajectory.py @@ -11,24 +11,41 @@ class TestTrajectory: - @pytest.mark.parametrize("robot_model", ["bez2"]) - @pytest.mark.parametrize("trajectory_name", ["fix_angle_test"]) - def test_all_trajectories(self, robot_model: str, trajectory_name: str): + def run_real_trajectory(self, robot_model: str, trajectory_name: str, real_time: bool): rospy.init_node("test") os.system( - "/bin/bash -c 'source /opt/ros/noetic/setup.bash && rosnode kill /robot1/soccer_strategy /robot1/soccer_pycontrol /robot1/soccer_trajectories'" + "/bin/bash -c 'source /opt/ros/noetic/setup.bash && rosnode kill /robot1/soccer_strategy " + "/robot1/soccer_pycontrol /robot1/soccer_trajectories'" ) + file_path = os.path.dirname(os.path.abspath(__file__)) config_path = f"{file_path}/../../../{robot_model}_description/config/motor_mapping.yaml" set_rosparam_from_yaml_file(param_path=config_path) rospy.set_param("robot_model", robot_model) - - if "DISPLAY" not in os.environ: - Trajectory.RATE = 10000 + # if "DISPLAY" not in os.environ: + # Trajectory.RATE = 10000 c = TrajectoryManager() rospy.init_node("test") msg = FixedTrajectoryCommand() msg.trajectory_name = trajectory_name msg.mirror = False c.command_callback(command=msg) - c.trajectory.run(real_time=True) + c.trajectory.run(real_time=real_time) + + @pytest.mark.parametrize("robot_model", ["bez2"]) + @pytest.mark.parametrize("trajectory_name", ["fix_angle_test"]) + @pytest.mark.parametrize("real_time", [True]) + def test_fixed_angles_trajectories(self, robot_model: str, trajectory_name: str, real_time: bool): + self.run_real_trajectory(robot_model, trajectory_name, real_time) + + @pytest.mark.parametrize("robot_model", ["bez2"]) + @pytest.mark.parametrize("trajectory_name", ["rightkick_2"]) + @pytest.mark.parametrize("real_time", [True]) + def test_getupfront_trajectories(self, robot_model: str, trajectory_name: str, real_time: bool): + self.run_real_trajectory(robot_model, trajectory_name, real_time) + + @pytest.mark.parametrize("robot_model", ["bez2"]) + @pytest.mark.parametrize("trajectory_name", ["fix_angle_test"]) + @pytest.mark.parametrize("real_time", [True]) + def test_all_trajectories(self, robot_model: str, trajectory_name: str, real_time: bool): + self.run_real_trajectory(robot_model, trajectory_name, real_time) diff --git a/soccer_trajectories/trajectories/bez2/fix_angle_test.csv b/soccer_trajectories/trajectories/bez2/fix_angle_test.csv index b8d65dc2d..7397e99fc 100644 --- a/soccer_trajectories/trajectories/bez2/fix_angle_test.csv +++ b/soccer_trajectories/trajectories/bez2/fix_angle_test.csv @@ -1,19 +1,19 @@ -time,0,1 -right_leg_motor_0,0,0 -left_leg_motor_0,0,0 -right_leg_motor_1,0,0 -left_leg_motor_1,0,0 -right_leg_motor_2,0,0 -left_leg_motor_2,0,0 -right_leg_motor_3,0,0 -left_leg_motor_3,0,0 -right_leg_motor_4,0,0 -left_leg_motor_4,0,0 -right_leg_motor_5,0,0 -left_leg_motor_5,0,0 -right_arm_motor_0,0,0 -left_arm_motor_0,0,0 -right_arm_motor_1,0,0 -left_arm_motor_1,1.5,0 -head_motor_0,0,0 -head_motor_1,0,0 +time,0,1.5,3 +right_leg_motor_0,0,0,0 +left_leg_motor_0,0,0,0 +right_leg_motor_1,0,0,0 +left_leg_motor_1,0,0,0 +right_leg_motor_2,0,0,0 +left_leg_motor_2,0,0,0 +right_leg_motor_3,0,0,0 +left_leg_motor_3,0,0,0 +right_leg_motor_4,0,0,0 +left_leg_motor_4,0,0,0 +right_leg_motor_5,0,0,0 +left_leg_motor_5,0,0,0 +right_arm_motor_0,0,0,0 +left_arm_motor_0,0,0,0 +right_arm_motor_1,0,0,0 +left_arm_motor_1,0,0,0 +head_motor_0,0,0,0 +head_motor_1,0,0,0 diff --git a/soccer_trajectories/trajectories/bez2/rightkick.csv b/soccer_trajectories/trajectories/bez2/rightkick.csv index 4e5e23a35..846b76243 100644 --- a/soccer_trajectories/trajectories/bez2/rightkick.csv +++ b/soccer_trajectories/trajectories/bez2/rightkick.csv @@ -1,6 +1,6 @@ time,0.5,1.5,2,2.5,5,8,8.1,8.3 left_leg_motor_0,0,0,0,0,0,0,0,0 -left_leg_motor_1,0.1,0.1,-0.15,-0.15,-0.25,-0.25,-0.25,-0.25 +left_leg_motor_1,0.1,0.1,-0.15,-0.15,-0.15,-0.15,-0.15,-0.15 left_leg_motor_2,0.25,0.25,0.25,0.25,0.25,0.25,0.25,0.25 left_leg_motor_3,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4 left_leg_motor_4,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2 @@ -16,3 +16,5 @@ right_leg_motor_5,-0.1,-0.1,-0.1,-0.1,-0.2,-0.2,-0.2,-0.2 right_arm_motor_0,0,0,0,0,1,1,-1,-1 right_arm_motor_1,2,2,2,2,2,2,2,2 comment,stance,x,lean to the side,x,lift leg up,x,kick,step to balance +head_motor_0,0,0,0,0,0,0,0,0 +head_motor_1,0,0,0,0,0,0,0,0 diff --git a/soccer_trajectories/trajectories/bez2/rightkick_2.csv b/soccer_trajectories/trajectories/bez2/rightkick_2.csv index 25f85c48c..07509ad7b 100644 --- a/soccer_trajectories/trajectories/bez2/rightkick_2.csv +++ b/soccer_trajectories/trajectories/bez2/rightkick_2.csv @@ -1,18 +1,20 @@ -time,0.69,0.7,0.75,1,1.2,1.4,1.41,1.7,2 +time,1,1.2,1.45,1.5,1.7,1.9,2.1,2.3,2.5 left_leg_motor_0,0,0,0,0,0,0,0,0,0 -left_leg_motor_1,0.1,0.1,-0.15,-0.15,-0.25,-0.25,-0.25,-0.25,0.1 +left_leg_motor_1,0.1,0.1,0.15,0.15,0.15,0.15,0.15,0.15,0.1 left_leg_motor_2,0.25,0.25,0.25,0.25,0.25,0.25,0.25,0.25,0.25 left_leg_motor_3,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4 left_leg_motor_4,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2 -left_leg_motor_5,-0.1,-0.1,0.15,0.15,0.25,0.25,0.25,0.25,-0.1 +left_leg_motor_5,-0.1,-0.1,0.15,0.15,0.25,0.25,0.4,0.4,-0.1 left_arm_motor_0,0,0,0,0,-1,-1,1,1,0 left_arm_motor_1,2,2,2,2,2,2,2,2,2 right_leg_motor_0,0,0,0,0,0,0,0,0,0 right_leg_motor_1,0.1,0.1,0.2,0.2,0.4,0.4,0.4,0.4,0.1 -right_leg_motor_2,0.25,0.25,0.25,0.25,0.3,0.3,1.5,1,0.25 -right_leg_motor_3,-0.4,-0.4,-0.4,-0.4,-1,-1,-1,-1,-0.4 +right_leg_motor_2,0.25,0.25,0.25,0.25,0.3,0.3,1,0.5,0.25 +right_leg_motor_3,-0.4,-0.4,-0.4,-0.4,-0.6,-0.8,-0.8,-0.8,-0.4 right_leg_motor_4,0.2,0.2,0.2,0.2,0.5,0.5,0,0,0.2 right_leg_motor_5,-0.1,-0.1,-0.1,-0.1,-0.2,-0.2,-0.2,-0.2,-0.1 right_arm_motor_0,0,0,0,0,1,1,-1,-1,0 right_arm_motor_1,2,2,2,2,2,2,2,2,2 comment,stance,x,lean to the side,x,lift leg up,x,kick,step to balance,stance +head_motor_0,0,0,0,0,0,0,0,0,0 +head_motor_1,0,0,0,0,0,0,0,0,0 diff --git a/soccerbot/CMakeLists.txt b/soccerbot/CMakeLists.txt index 672d591d4..a99ecb959 100644 --- a/soccerbot/CMakeLists.txt +++ b/soccerbot/CMakeLists.txt @@ -5,7 +5,6 @@ find_package( catkin REQUIRED soccer_common - soccer_webots soccer_msgs soccer_trajectories soccer_object_detection diff --git a/soccerbot/launch/modules/sensors.launch b/soccerbot/launch/modules/sensors.launch index fac9a6cad..d7fad3599 100644 --- a/soccerbot/launch/modules/sensors.launch +++ b/soccerbot/launch/modules/sensors.launch @@ -2,33 +2,53 @@ - - + + - + - - - - + + + + + - - - - - - - - + + + + + + + + + + + + + + - - + + + + + + + + + + + + + + + diff --git a/soccerbot/package.xml b/soccerbot/package.xml index ad6fbd8d1..8d045cf04 100644 --- a/soccerbot/package.xml +++ b/soccerbot/package.xml @@ -38,7 +38,7 @@ soccer_trajectories soccer_firmware_interface - soccer_webots + soccer_msgs python3-protobuf