DO NOT MERGE: Changes from CIRC competition #335

abhaybd · 2024-08-14T19:17:04Z

This PR consists of hotfixes and changes implemented during the CIRC 2024 competition. As a result, much of this code consists of quick fixes/hacks that shouldn't be merged, but should rather be evaluated if we need it, and should be cleaned up and merged as a separate PR.

I will go through the code noting why sections of code were fixed, to help with the cleaning up process.

Formatting

abhaybd · 2024-08-14T19:19:27Z

camera-config/PanoCameraCalibration.yml

+calib_info:
+   calibration_time: "Sat 01 May 2021 01:54:16 PM PDT"
+   board_width: 11
+   board_height: 8
+   square_size: 20.
+   flags: 0
+   avg_reprojection_error: 4.5744250014767379e-01
+intrinsic_params:
+   image_width: 640
+   image_height: 480
+   camera_matrix: !!opencv-matrix
+      rows: 3
+      cols: 3
+      dt: d
+      data: [ 6.4936764773277400e+02, 0., 3.3546152305967058e+02, 0.,
+          6.4776804982533793e+02, 2.4785660889077488e+02, 0., 0., 1. ]
+   distortion_coefficients: !!opencv-matrix
+      rows: 5
+      cols: 1
+      dt: d
+      data: [ -4.5670452858419575e-01, 2.1512832208218641e-01,
+          -1.1179480533495888e-03, -5.7450430930239835e-04, 0. ]
+extrinsic_params: !!opencv-matrix
+  rows: 4
+  cols: 4


These are all unused. We should redo how we handle camera configuration, especially considering the changes in Camera.cpp

abhaybd · 2024-08-14T19:19:50Z

camera-config/DrillCameraCalibration.yml

+calib_info:
+   calibration_time: "Sat 01 May 2021 01:54:16 PM PDT"
+   board_width: 11
+   board_height: 8
+   square_size: 20.
+   flags: 0
+   avg_reprojection_error: 4.5744250014767379e-01
+intrinsic_params:
+   image_width: 640
+   image_height: 480
+   camera_matrix: !!opencv-matrix
+      rows: 3
+      cols: 3
+      dt: d
+      data: [ 6.4936764773277400e+02, 0., 3.3546152305967058e+02, 0.,
+          6.4776804982533793e+02, 2.4785660889077488e+02, 0., 0., 1. ]
+   distortion_coefficients: !!opencv-matrix
+      rows: 5
+      cols: 1
+      dt: d
+      data: [ -4.5670452858419575e-01, 2.1512832208218641e-01,
+          -1.1179480533495888e-03, -5.7450430930239835e-04, 0. ]
+extrinsic_params: !!opencv-matrix
+  rows: 4
+  cols: 4
+  dt: d


Same as other camera, unused. See note on other camera config file.

abhaybd · 2024-08-14T19:20:14Z

src/CAN/CANMotor.h

@@ -136,6 +136,8 @@ void setMotorPower(deviceserial_t serial, int16_t power);
 */
 void setMotorPIDTarget(deviceserial_t serial, int32_t target);

+void setMotorPIDMaxPower(deviceserial_t serial, uint16_t maxPower);


This method restricts the maximum power that the motor is allowed to apply when running PID. Add documentation! Also, maybe add an overload with a double parameter for power in the range [0,1]?

abhaybd · 2024-08-14T19:21:28Z

src/Constants.h

-	SEGMENT_LENGTHS{{robot::types::motorid_t::shoulder, 0.3848608},
-					{robot::types::motorid_t::elbow, 0.461264}};
+	SEGMENT_LENGTHS{{robot::types::motorid_t::shoulder, 0.394},
+					{robot::types::motorid_t::elbow, 0.749}};


Note that the elbow measurement goes all the way from the elbow joint to the tip of the fingers of the hand.

abhaybd · 2024-08-14T19:22:38Z

src/TunePID.cpp

+		can::motor::setLimitSwitchLimits(serial, mDegToPulses(enc_params.limitSwitchLow),
+										 mDegToPulses(enc_params.limitSwitchHigh));


The parameters to setLimitSwitchLimits() should be in encoder ticks, not mdegs. This used to work before, only because it was incorrect on both sides (firmware and software) but disagreed with documentation. This has been updated on both sides.

abhaybd · 2024-08-14T19:39:31Z

src/world_interface/real_world_interface.cpp

@@ -53,7 +53,7 @@ const kinematics::DiffDriveKinematics& driveKinematics() {

 namespace {
 // map that associates camera id to the camera object
-std::unordered_map<CameraID, std::shared_ptr<cam::Camera>> cameraMap;
+std::unordered_map<CameraID, std::weak_ptr<cam::Camera>> cameraMap;


Maintain a weak pointer here so reference counting works. When the number of strong references (all held by client code) drops to 0 , the camera will be cleaned up.

abhaybd · 2024-08-14T19:41:19Z

src/world_interface/real_world_interface.cpp

+		// inverted because of weirdness
+		can::motor::setLimitSwitchLimits(serial, mDegToPulses(-enc_params.limitSwitchLow),
+										 mDegToPulses(-enc_params.limitSwitchHigh));


This is due to some quaddec inversion weirdness in firmware that I don't fully understand. Ask Casey and/or Michael. This should be fixed in firmware and we shouldn't have to negate the parameters.

abhaybd · 2024-08-14T19:42:31Z

src/world_interface/real_world_interface.cpp

@@ -96,33 +100,35 @@ void initMotors() {
 	}
 }

-void openCamera(CameraID camID, const char* cameraPath) {
+std::shared_ptr<cam::Camera> openCamera(CameraID camID, const std::string& cameraPath) {


As I said, the Jetson can only open 2 cameras, so we're opening cameras as necessary (instead of at the beginning) and using reference counting to figure out when to close it. This involves weak and shared pointers, which should be pretty intuitive.

abhaybd · 2024-08-14T19:43:52Z

src/world_interface/world_interface.h

+class CameraHandle {
+public:
+	CameraHandle(std::shared_ptr<cam::Camera> cam) : camera(cam) {}
+private:
+	std::shared_ptr<cam::Camera> camera;
+};


This is a dumb class that is used for reference counting without letting client code have access to the actual camera object. Is this the right place for it?

abhaybd · 2024-08-14T19:44:09Z

src/world_interface/world_interface.h

@@ -87,6 +92,8 @@ bool isEmergencyStopped();
 */
 std::unordered_set<types::CameraID> getCameras();

+std::shared_ptr<types::CameraHandle> openCamera(types::CameraID camera);


Geeoon · 2024-10-07T20:34:45Z

src/Constants.cpp

+const std::unordered_map<robot::types::CameraID, std::string> CAMERA_CONFIG_PATHS = {
+   {MAST_CAMERA_ID, "../camera-config/MastCameraCalibration.yml"},
+   {FOREARM_CAMERA_ID, "../camera-config/WristCameraCalibration.yml"},
+   {HAND_CAMERA_ID, "../camera-config/HandCameraCalibration.yml"},
+   {PANO_CAMERA_ID, "../camera-config/PanoCameraCalibration.yml"},
+   {DRILL_CAMERA_ID, "../camera-config/DrillCameraCalibration.yml"}
+};


Useless, as mentioned for PanoCameraCalibration.yml

Geeoon · 2024-10-07T20:45:17Z

src/world_interface/simulator_interface.cpp

+std::shared_ptr<types::CameraHandle> openCamera(CameraID cameraID) {
+	return nullptr;
+}


Why is the old openCamera function returning nullptr?

huttongrabiel · 2024-11-05T05:02:51Z

closing for abhays mental health

abhaybd and others added 9 commits August 8, 2024 01:45

Add hand actuator

a593fc6

Try camera workaround

362f0dd

Fix camera bug

96939b3

Bix fugs

93a906b

dirty nasty hacks

fda5020

Add cameras, more wheel juice

c59c82e

try again to add aruco

048ee4a

Formatting

IK fixes

723c6fd

Fix id

bda1882

abhaybd assigned huttongrabiel Aug 14, 2024

abhaybd commented Aug 14, 2024

View reviewed changes

Geeoon reviewed Oct 7, 2024

View reviewed changes

huttongrabiel closed this Nov 5, 2024

huttongrabiel deleted the circ-24 branch November 5, 2024 05:02

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

DO NOT MERGE: Changes from CIRC competition #335

DO NOT MERGE: Changes from CIRC competition #335

abhaybd commented Aug 14, 2024

abhaybd Aug 14, 2024

abhaybd Aug 14, 2024

abhaybd Aug 14, 2024

abhaybd Aug 14, 2024

abhaybd Aug 14, 2024

abhaybd Aug 14, 2024

abhaybd Aug 14, 2024

abhaybd Aug 14, 2024

abhaybd Aug 14, 2024

abhaybd Aug 14, 2024

Geeoon Oct 7, 2024

Geeoon Oct 7, 2024

huttongrabiel commented Nov 5, 2024

		can::motor::setLimitSwitchLimits(serial, mDegToPulses(enc_params.limitSwitchLow),
		mDegToPulses(enc_params.limitSwitchHigh));

DO NOT MERGE: Changes from CIRC competition #335

DO NOT MERGE: Changes from CIRC competition #335

Conversation

abhaybd commented Aug 14, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

huttongrabiel commented Nov 5, 2024