Add normalize functionality to set_setpoint

huskyroboticsteam · Oct 3, 2023 · 9d0cdfe · 9d0cdfe
1 parent affcec0
commit 9d0cdfe
Showing 1 changed file with 2 additions and 1 deletion.
diff --git a/src/control/PlanarArmController.h b/src/control/PlanarArmController.h
@@ -47,7 +47,8 @@ template <unsigned int N> class PlanarArmController {
 	void set_setpoint(const navtypes::Vectord<N>& targetJointPos) {
 		Eigen::Vector2d newSetPoint = kin.jointPosToEEPos(targetJointPos);
 		std::lock_guard<std::mutex> lock(mutex);
-		setpoint = newSetPoint;
+		double radius = kin.getSegLens().sum() * safetyFactor;
+		setpoint = normalizeVectorWithinRadius(newSetPoint, radius);
 	}
 
 	/**