diff --git a/CMakeLists.txt b/CMakeLists.txt index 1b18f1a9c..e02e2f0a7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -67,15 +67,19 @@ set(MROVER_ROS_DEPENDENCIES tf2 tf2_ros tf2_geometry_msgs + actionlib_msgs ) extract_filenames(msg/*.msg MROVER_MESSAGE_FILES) extract_filenames(srv/*.srv MROVER_SERVICE_FILES) +extract_filenames(action/*.action MROVER_ACTION_FILES) + set(MROVER_MESSAGE_DEPENDENCIES std_msgs sensor_msgs + actionlib_msgs ) set(MROVER_PARAMETERS @@ -98,7 +102,7 @@ if (ZED_FOUND) set(CMAKE_CUDA_STANDARD 17) set(CMAKE_CUDA_STANDARD_REQUIRED ON) set(CMAKE_CUDA_SEPARABLE_COMPILATION ON) - set(CMAKE_CUDA_FLAGS "--diag-suppress 108,68") + set(CMAKE_CUDA_FLAGS "--diag-suppress 20012") # Jetson Xavier NX/AGX has Volta 7.2 architecture # Perception Laptop (A4000, Quadro version of RTX 3080) has Ampere 8.6 architecture set(CMAKE_CUDA_ARCHITECTURES 72 86) @@ -123,6 +127,8 @@ add_message_files(FILES ${MROVER_MESSAGE_FILES}) add_service_files(FILES ${MROVER_SERVICE_FILES}) +add_action_files(DIRECTORY action FILES ${MROVER_ACTION_FILES}) + generate_messages(DEPENDENCIES ${MROVER_MESSAGE_DEPENDENCIES}) generate_dynamic_reconfigure_options(${MROVER_PARAMETERS}) diff --git a/action/ArmAction.action b/action/ArmAction.action new file mode 100644 index 000000000..9cc4f5321 --- /dev/null +++ b/action/ArmAction.action @@ -0,0 +1,3 @@ +string name +--- +--- diff --git a/ansible/roles/build/files/profiles/ci/config.yaml b/ansible/roles/build/files/profiles/ci/config.yaml index d14216f40..c0346527b 100644 --- a/ansible/roles/build/files/profiles/ci/config.yaml +++ b/ansible/roles/build/files/profiles/ci/config.yaml @@ -5,9 +5,9 @@ catkin_make_args: [ ] cmake_args: - -DCMAKE_BUILD_TYPE=Release - -DCMAKE_C_COMPILER=clang-16 - - -DCMAKE_CXX_FLAGS=-pipe + - -DCMAKE_CXX_FLAGS=-pipe -fgnuc-version=13.1.0 - -DCMAKE_CXX_COMPILER=clang++-16 - - -DCMAKE_CUDA_HOST_COMPILER=clang++-16 + - -DCMAKE_CUDA_HOST_COMPILER=g++-9 - -DMROVER_IS_CI=ON - -Wno-dev devel_layout: linked diff --git a/ansible/roles/build/files/profiles/debug/config.yaml b/ansible/roles/build/files/profiles/debug/config.yaml index 71005790a..434ae5ae9 100644 --- a/ansible/roles/build/files/profiles/debug/config.yaml +++ b/ansible/roles/build/files/profiles/debug/config.yaml @@ -5,11 +5,11 @@ catkin_make_args: [ ] cmake_args: - -DCMAKE_BUILD_TYPE=Debug - -DCMAKE_C_COMPILER=clang-16 - - -DCMAKE_CXX_FLAGS=-pipe + - -DCMAKE_CXX_FLAGS=-pipe -fgnuc-version=13.1.0 - -DCMAKE_CXX_COMPILER=clang++-16 - -DCMAKE_CXX_COMPILER_LAUNCHER=ccache - -DCMAKE_CUDA_COMPILER_LAUNCHER=ccache - - -DCMAKE_CUDA_HOST_COMPILER=clang++-16 + - -DCMAKE_CUDA_HOST_COMPILER=g++-9 - -Wno-dev devel_layout: linked devel_space: devel diff --git a/ansible/roles/build/files/profiles/release/config.yaml b/ansible/roles/build/files/profiles/release/config.yaml index f0c29e538..ef35c5cb7 100644 --- a/ansible/roles/build/files/profiles/release/config.yaml +++ b/ansible/roles/build/files/profiles/release/config.yaml @@ -5,11 +5,11 @@ catkin_make_args: [ ] cmake_args: - -DCMAKE_BUILD_TYPE=Release - -DCMAKE_C_COMPILER=clang-16 - - -DCMAKE_CXX_FLAGS=-march=native -pipe + - -DCMAKE_CXX_FLAGS=-march=native -pipe -fgnuc-version=13.1.0 - -DCMAKE_CXX_COMPILER=clang++-16 - -DCMAKE_CXX_COMPILER_LAUNCHER=ccache - -DCMAKE_CUDA_COMPILER_LAUNCHER=ccache - - -DCMAKE_CUDA_HOST_COMPILER=clang++-16 + - -DCMAKE_CUDA_HOST_COMPILER=g++-9 - -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON - -Wno-dev devel_layout: linked diff --git a/ansible/roles/build/tasks/main.yml b/ansible/roles/build/tasks/main.yml index 6d91a9651..c2c6e022f 100644 --- a/ansible/roles/build/tasks/main.yml +++ b/ansible/roles/build/tasks/main.yml @@ -97,6 +97,7 @@ - libgstreamer1.0-dev - libgstreamer-plugins-base1.0-dev - vainfo + - x264 - name: Install Local APT Packages become: True diff --git a/config/esw.yaml b/config/esw.yaml index f0a78c30f..338fc08ef 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -43,7 +43,7 @@ can: id: 0x15 - name: "joint_a" bus: 1 - id: 0x20 + id: 0x20 - name: "joint_b" bus: 1 id: 0x21 @@ -64,10 +64,10 @@ can: id: 0x26 - name: "mast_gimbal_y" bus: 3 - id: 0x28 + id: 0x27 - name: "mast_gimbal_z" bus: 3 - id: 0x27 + id: 0x28 - name: "sa_x" bus: 1 id: 0x29 @@ -122,10 +122,10 @@ brushless_motors: max_velocity: 20.0 max_torque: 5.0 joint_a: - # 1236.8475 rad/meter, or 196.850393476 rev/meter, and we want a max vel of 40 meter/s. + # 1236.8475 rad/meter, or 196.850393476 rev/meter, and we want a max vel of 40 meter/s. # gear ratio is currently at 0.005080 revolutions = 1 meter - min_velocity: -0.10 # this means -0.10 meters per second. - max_velocity: 0.10 + min_velocity: -0.05 # this means -0.10 meters per second. + max_velocity: 0.05 limit_0_present: true limit_1_present: true limit_0_enabled: true @@ -144,11 +144,11 @@ brushless_motors: max_backward_pos: 0.0 max_torque: 20.0 joint_c: - min_velocity: -0.08 # in terms of output - max_velocity: 0.08 # max output shaft speed: 5 rpm (for now) + min_velocity: -0.03 # in terms of output + max_velocity: 0.03 # max output shaft speed: 5 rpm (for now) min_position: -0.125 max_position: 0.30 # 220 degrees of motion is the entire range - max_torque: 20.0 + max_torque: 200.0 joint_de_0: min_velocity: -5.0 max_velocity: 5.0 @@ -595,4 +595,4 @@ auton_led_driver: baud: 115200 science: - shutoff_temp: 50.0f \ No newline at end of file + shutoff_temp: 50.0f diff --git a/config/moteus/joint_c.cfg b/config/moteus/joint_c.cfg index c475f8708..761535fc1 100644 --- a/config/moteus/joint_c.cfg +++ b/config/moteus/joint_c.cfg @@ -1363,9 +1363,9 @@ servo.adc_cur_cycles 2 servo.adc_aux_cycles 47 servo.pid_dq.kp 0.060606 servo.pid_dq.ki 102.321388 -servo.pid_position.kp 14.000000 +servo.pid_position.kp 800.000000 servo.pid_position.ki 0.000000 -servo.pid_position.kd 1.450000 +servo.pid_position.kd 50.000000 servo.pid_position.iratelimit -1.000000 servo.pid_position.ilimit 0.000000 servo.pid_position.max_desired_rate 0.000000 diff --git a/config/teleop.yaml b/config/teleop.yaml index 8c05ad09a..1b8c68952 100644 --- a/config/teleop.yaml +++ b/config/teleop.yaml @@ -6,34 +6,24 @@ teleop: joint_a: multiplier: -1 slow_mode_multiplier: 0.5 - invert: True - xbox_index: 0 joint_b: multiplier: 1 slow_mode_multiplier: 1 - invert: False - xbox_index: 1 joint_c: - multiplier: -1 + multiplier: 1 slow_mode_multiplier: 0.6 - invert: False - xbox_index: 3 joint_de_pitch: - multiplier: 0.5 + multiplier: -1 slow_mode_multiplier: 1 - invert: True joint_de_roll: - multiplier: -0.5 + multiplier: 1 slow_mode_multiplier: 1 - invert: True allen_key: multiplier: 1 slow_mode_multiplier: 1 - invert: False gripper: multiplier: 1 slow_mode_multiplier: 1 - invert: True sa_controls: sa_x: @@ -67,21 +57,32 @@ teleop: enabled: true multiplier: 1 twist: - multiplier: 0.7 + multiplier: 0.6 + tilt: + multiplier: 1 xbox_mappings: - left_js_x: 0 - left_js_y: 1 - right_js_x: 2 - right_js_y: 3 - left_trigger: 6 - right_trigger: 7 + left_x: 0 + left_y: 1 + right_x: 2 + right_y: 3 a: 0 b: 1 x: 2 y: 3 left_bumper: 4 right_bumper: 5 + left_trigger: 6 + right_trigger: 7 + back: 8 + forward: 9 + left_stick_click: 10 + right_stick_click: 11 + dpad_up: 12 + dpad_down: 13 + dpad_left: 14 + dpad_right: 15 + home: 16 joystick_mappings: left_right: 0 @@ -119,6 +120,6 @@ teleop: bar: ["/tf_static", "/rosout", "/tf"] ik_multipliers: - x: 1 - y: 1 - z: 1 \ No newline at end of file + x: 0.1 + y: 0.1 + z: 0.1 \ No newline at end of file diff --git a/launch/basestation.launch b/launch/basestation.launch index b5039bcbe..47f7dd691 100644 --- a/launch/basestation.launch +++ b/launch/basestation.launch @@ -4,14 +4,17 @@ + + + - - - + + + diff --git a/launch/esw_base.launch b/launch/esw_base.launch index e2ccf7d2d..0bcbf81c8 100644 --- a/launch/esw_base.launch +++ b/launch/esw_base.launch @@ -17,19 +17,11 @@ output="screen"> - - - + @@ -47,15 +39,23 @@ -
@@ -113,7 +106,9 @@ import MotorAdjust from './MotorAdjust.vue' import OdometryReading from './OdometryReading.vue' import SAArmControls from './SAArmControls.vue' import { disableAutonLED, quaternionToMapAngle } from '../utils.js' -import { mapState } from 'vuex' +import { mapState, mapActions } from 'vuex' + +let interval: number export default { components: { @@ -182,10 +177,26 @@ export default { this.moteusState.error = msg.error this.moteusState.limit_hit = msg.limit_hit } + else if (msg.type == 'nav_sat_fix') { + this.odom.latitude_deg = msg.latitude + this.odom.longitude_deg = msg.longitude + this.odom.altitude = msg.altitude + } + else if (msg.type == 'bearing') { + this.odom.bearing_deg = quaternionToMapAngle(msg.rotation) + } } }, - created: function () {} + methods: { + ...mapActions('websocket', ['sendMessage']), + }, + + created: function () { + interval = setInterval(() => { + this.sendMessage({ type: 'bearing' }) + }, 1000) + } } diff --git a/src/teleoperation/frontend/src/router/index.ts b/src/teleoperation/frontend/src/router/index.ts index f34e022f1..c8681c3d0 100644 --- a/src/teleoperation/frontend/src/router/index.ts +++ b/src/teleoperation/frontend/src/router/index.ts @@ -4,6 +4,7 @@ import DMESTask from '../components/DMESTask.vue' import AutonTask from '../components/AutonTask.vue' import ISHTask from '../components/ISHTask.vue' import SATask from '../components/SATask.vue' +import Rover3D from '../components/Rover3D.vue' const routes = [ { @@ -41,7 +42,13 @@ const routes = [ path: '/ISHTask', name: 'ISHTask', component: ISHTask - } + }, + { + path: "/Control", + name: "Control", + component: Rover3D, + }, + ] const router = createRouter({ history: createWebHistory(), diff --git a/src/teleoperation/frontend/src/rover_three.js b/src/teleoperation/frontend/src/rover_three.js new file mode 100644 index 000000000..ac83a7d02 --- /dev/null +++ b/src/teleoperation/frontend/src/rover_three.js @@ -0,0 +1,162 @@ +import * as THREE from 'three'; +import { FBXLoader } from 'three/examples/jsm/loaders/FBXLoader' +import { TrackballControls } from 'three/addons/controls/TrackballControls.js'; + +export function threeSetup(containerId) { + // Get the container where the scene should be placed + const container = document.getElementById(containerId); + + const canvas = { + width: container.clientWidth, + height: container.clientHeight + } + + const scene = new THREE.Scene(); + const camera = new THREE.PerspectiveCamera(75, canvas.width / canvas.height, 0.1, 1000); + camera.up.set(0, 0, 1); + + const directionalLight = new THREE.DirectionalLight(0xffffff, 0.5); + directionalLight.position.set(1, 2, 3); + scene.add(directionalLight); + + const renderer = new THREE.WebGLRenderer(); + renderer.setSize(canvas.width, canvas.height); + container.appendChild(renderer.domElement); // append it to your specific HTML element + + scene.background = new THREE.Color(0xcccccc); + + //controls to orbit around model + var controls = new TrackballControls(camera, renderer.domElement); + controls.rotateSpeed = 1.0; + controls.zoomSpeed = 1.2; + controls.panSpeed = 0.8; + controls.noZoom = false; + controls.noPan = false; + controls.staticMoving = true; + controls.dynamicDampingFactor = 0.3; + + const joints = [ + { + name: "chassis", + file: "rover_chassis.fbx", + translation: [0.164882, -0.200235, 0.051497], + rotation: [0, 0, 0], + }, + { + name: "a", + file: "arm_a.fbx", + translation: [0.034346, 0, 0.049024], + rotation: [0, 0, 0], + }, + { + name: "b", + file: "arm_b.fbx", + translation: [0.534365, 0, 0.009056], + rotation: [0, 0, 0], + }, + { + name: "c", + file: "arm_c.fbx", + translation: [0.546033, 0, 0.088594], + rotation: [0, 0, 0], + }, + { + name: "d", + file: "arm_d.fbx", + translation: [0.044886, 0, 0], + rotation: [0, 0, 0], + }, + { + name: "e", + file: "arm_e.fbx", + translation: [0.044886, 0, 0], + rotation: [0, 0, 0], + } + ] + + const loader = new FBXLoader(); + for (let i = 0; i < joints.length; ++i) { + loader.load('/meshes/' + joints[i].file, (fbx) => { + // Traverse the loaded scene to find meshes or objects + fbx.traverse((child)=> { + if (child.isMesh) { + if(joints[i].name == "d" || joints[i].name == "e") { + child.material = new THREE.MeshStandardMaterial({color: 0x00ff00}); + } + else child.material = new THREE.MeshStandardMaterial({color: 0xffffff}); + scene.add(child); + child.scale.set(1, 1, 1); + child.position.set(0, 0, 0); + } + }); + }, + (xhr) => { + console.log((xhr.loaded / xhr.total) * 100 + '% loaded') + }, + (error) => { + console.log(error) + }); + } + + camera.position.x = 2; + camera.position.y = 2; + camera.position.z = 2; + camera.lookAt(new THREE.Vector3(0, 0, 0)); + + const targetCube = new THREE.Mesh( + new THREE.BoxGeometry( 0.1, 0.1, 0.1 ), + new THREE.MeshBasicMaterial({color: 0x00ff00}) + ); + scene.add( targetCube ); + + function animate() { + requestAnimationFrame(animate); + controls.update(); + renderer.render(scene, camera); + } + + animate(); + + function fk(positions) { + + let cumulativeMatrix = new THREE.Matrix4(); + + cumulativeMatrix.makeTranslation(new THREE.Vector3(0, 0, 0.439675)); //makes meshes relative to base_link + + for(let i = 0; i < joints.length; ++i){ + let mesh = scene.getObjectByName(joints[i].name); + + let localMatrix = new THREE.Matrix4(); + + let rotationAngle = positions[i]; // Angle for this joint + if(joints[i].name == "chassis") { + localMatrix.makeTranslation(0,rotationAngle,0); + } + else { + localMatrix.makeRotationY(rotationAngle); + } + + let offset = new THREE.Vector3().fromArray(joints[i].translation); + + localMatrix.setPosition( + new THREE.Vector3().setFromMatrixPosition(localMatrix).add(offset) + ); + + mesh.matrixAutoUpdate = false; + mesh.matrix = cumulativeMatrix.clone(); + + cumulativeMatrix.multiply(localMatrix); + } + } + + function ik(target) { + let quaternion = new THREE.Quaternion(...target.quaternion); + targetCube.position.set(...target.position); + targetCube.setRotationFromQuaternion(quaternion); + } + + // Return an object with the updateJointAngles() function + return { + fk, ik + } +}; diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index 3aedbbc29..f79b800bb 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -204,7 +204,7 @@ - +