updating from tests

.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

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
+```

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

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'

soccer_firmware_interface/config/bez2.yaml

@@ -1,117 +1,135 @@
 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"
   angle_zero: 180
   angle_max: 360
   angle_min: 0
   flipped: "true"
+  initial_state: -0.05
 left_leg_motor_2:
   id: 6
   type: "xm430"
   angle_zero: 180
   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"
   angle_zero: 180
   angle_max: 360
   angle_min: 0
   flipped: "true"
+  initial_state: 0.0
 right_leg_motor_0:
   id: 10
   type: "xm430"
   angle_zero: 180
   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"
   angle_zero: 180
   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
   type: "mx64"
   angle_zero: 180
   angle_max: 360
   angle_min: 0
+  initial_state: 0.0
 left_arm_motor_0:
   id: 2
   type: "xm430"
   angle_zero: 180
   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"
   angle_zero: 180
   angle_max: 360
   angle_min: 0
   flipped: "true"
+  initial_state: 1.5
 head_motor_0:
   id: 16
   type: "ax12"
   angle_zero: 150
   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

soccer_hardware/launch/soccer_hardware.launch

@@ -1,6 +1,6 @@
 <launch>
     <arg name="robot_name" default="robot1" />
-    <arg name="test_walking" default="false" />
+    <arg name="test_walking" default="true" />
 
     <node launch-prefix="/usr/bin/nice --adjustment=-20" name="soccer_hardware" pkg="soccer_hardware" type="soccer_hardware.py" output="screen" cwd="node">
         <param name="port" value="/dev/ttyACM" />
@@ -25,4 +25,14 @@
         <param name="output_frame" value="$(arg robot_name)/odom" />
         <param name="base_footprint_frame" value="$(arg robot_name)/base_footprint" />
     </node>
+
+    <node
+        pkg="rosbag"
+        type="record"
+        name="rosbag_bez1"
+        args="-O $(find soccerbot)/bags/bez1_back.bag
+        /robot1/imu_raw
+        /robot1/imu_filtered
+    "
+    />
 </launch>

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()