Update representation of links in the SerialLink class

Control/SerialDynCtrl.h

@@ -9,7 +9,7 @@ class SerialDynCtrl : public SerialKinCtrl
 		bool set_gains(const float &proportional, const float &derivative);
 
 		// Get Functions
-virtual	Eigen::VectorXf get_joint_control(const float &time);		// Get the control to track the joint trajectory
+//virtual	Eigen::VectorXf get_joint_control(const float &time);		// Get the control to track the joint trajectory
 
 	private:
 		float kp = 100;							// Proportional gain
@@ -19,14 +19,14 @@ virtual	Eigen::VectorXf get_joint_control(const float &time);		// Get the contro
 
 };											// Semicolon needed after class declaration
 
-/******************** Create a dynamic-level controller for a SerialLink object ********************/
+/******************** Create a dynamic-level controller for a SerialLink object ********************
 SerialDynCtrl::SerialDynCtrl(const SerialLink &serial) : SerialKinCtrl(serial)
 {
 	this->K.resize(6,6); this->K.setIdentity()*25;				// Set default values for Cartesian stiffness
 	this->D.resize(6,6); this->D.setIdentity()*10;				// Cartesian damping >= 2*sqrt(K)
 }
 
-/******************** Set the gains for joint feedback control ********************/
+/******************** Set the gains for joint feedback control ********************
 bool SerialDynCtrl::set_gains(const float &proportional, const float &derivative)
 {
 	// Check that the inputs are sound
@@ -52,7 +52,7 @@ bool SerialDynCtrl::set_gains(const float &proportional, const float &derivative
 	}	
 }
 
-/******************** Get the joint torque to track a trajectory ********************/
+/******************** Get the joint torque to track a trajectory ********************
 Eigen::VectorXf SerialDynCtrl::get_joint_control(const float &time)
 {
 	Eigen::VectorXf q_d(this->n), qdot_d(this->n), qddot_d(this->n);			// Desired joint state
@@ -65,3 +65,4 @@ Eigen::VectorXf SerialDynCtrl::get_joint_control(const float &time)
 	// tau = M*qddot + C*qdot + g
 	return this->robot.get_inertia()*qddot + this->robot.get_coriolis*qdot + this->robot.get_gravity_torque();				
 }
+*/

Control/SerialKinCtrl.h

@@ -18,7 +18,7 @@ class SerialKinCtrl
 		Eigen::VectorXf get_cartesian_control(const float &time);		// Control to track Cartesian trajectory
 		Eigen::MatrixXf get_inverse(const Eigen::MatrixXf &A);		// Get the inverse of the given matrix
 		Eigen::MatrixXf get_inverse(const Eigen::MatrixXf &A, const Eigen::MatrixXf &W); // Get the weighted inverse of a given matrix
-virtual	Eigen::VectorXf get_joint_control(const float &time);		// Control to track joint trajectory
+		Eigen::VectorXf get_joint_control(const float &time);		// Control to track joint trajectory
 		Eigen::VectorXf get_pose_error(const Eigen::Isometry3f &desired, const Eigen::Isometry3f &actual);
 
 	protected: // SerialDynCtrl can access these
@@ -31,11 +31,12 @@ virtual	Eigen::VectorXf get_joint_control(const float &time);		// Control to tra
 		MultiPointTrajectory jointTrajectory;					// Joint trajectory object		
 
 	private:
-		float k = 1.0;								// Proportional gain on position error
+		float k = 1.0;								// Proportional gain on pose error
 
 		// Functions
-		Eigen::MatrixXf get_joint_limit_weighting();				// For redundant manipulators
-		bool scale_velocity_vector(Eigen::VectorXf &vec, const Eigen::VectorXf ref);
+		bool scale_velocity_vector(Eigen::VectorXf &vec, const Eigen::VectorXf ref);			// Ensure feasibility with RMRC
+		Eigen::MatrixXf get_joint_limit_weighting();							// For redundant manipulators
+		Eigen::VectorXf optimise_manipulability(const float &scalar, const Eigen::MatrixXf &J);	// Redundant task
 
 };											// Semicolon needed after class declaration
 
@@ -56,8 +57,8 @@ bool SerialKinCtrl::set_joint_target(Eigen::VectorXf &target)
 {
 	if(target.size() != this->n)
 	{
-		std::cout << "ERROR: SerialKinCtrl::set_joint_target() : Input vector has "
-			<< target.size() << " elements, but the robot has " << n << " joints!"
+		std::cerr << "[ERROR] [SERIALKINCTRL] set_joint_target() : Input vector had "
+			<< target.size() << " elements, but the robot has " << this->n << " joints."
 			<< " Joint target has not been set." << std::endl;
 
 		return false;
@@ -105,7 +106,7 @@ bool SerialKinCtrl::set_feedback_gain(const float &gain)
 {
 	if(gain < 0)
 	{
-		std::cerr << "[ERROR][SERIALKINCTRL] set_feedback_gain() : Value cannot be negative." << std::endl;
+		std::cerr << "[ERROR] [SERIALKINCTRL] set_feedback_gain() : Value cannot be negative." << std::endl;
 		std::cerr << " Input value: " << gain << std::endl;
 		return false;
 	}
@@ -121,7 +122,7 @@ bool SerialKinCtrl::set_target_pose(Eigen::Isometry3f &target, float &time)
 {
 	if(time <= 0)
 	{
-		std::cerr << "[ERROR][SERIALKINCTRL] set_target_pose() : Time must be greater than zero." << std::endl;
+		std::cerr << "[ERROR] [SERIALKINCTRL] set_target_pose() : Time must be greater than zero." << std::endl;
 		std::cerr << "Input time: " << time << std::endl;
 		return false;
 	}
@@ -137,7 +138,7 @@ bool SerialKinCtrl::set_target_pose(Eigen::Isometry3f &target, float &time)
 		maxSpeed = 1.0;							// m/s
 		if(distance / time > maxSpeed)
 		{
-			std::cerr << "[WARNING][SERIALKINCTRL] set_target_pose() : Linear velocity exceeds "
+			std::cerr << "[WARNING] [SERIALKINCTRL] set_target_pose() : Linear velocity exceeds "
 				<< maxSpeed << " m/s! Increasing the trajectory time..." << std::endl;
 			time = distance/maxSpeed;
 		}
@@ -148,8 +149,8 @@ bool SerialKinCtrl::set_target_pose(Eigen::Isometry3f &target, float &time)
 		if(distance > M_PI) distance = 2*M_PI - distance;			// Shortest path (should be corrected in trajectory object)
 		if(distance / time > maxSpeed)
 		{
-			std::cerr << "[WARNING][SERIALKINCTRL] set_target_pose() : Angular velocity exceeds "
-				<< maxSpeed*9.54 << " RPM! Increasing the trajectory time..." << std::endl;
+			std::cerr << "[WARNING] [SERIALKINCTRL] set_target_pose() : Angular velocity exceeds "
+				<< maxSpeed*30/M_PI << " RPM! Increasing the trajectory time..." << std::endl;
 			time = distance/maxSpeed;
 		}
 
@@ -173,29 +174,44 @@ bool SerialKinCtrl::set_target_poses(const std::vector<Eigen::Isometry3f> &targe
 Eigen::VectorXf SerialKinCtrl::get_cartesian_control(const float &time)
 {
 	// Get the desired state for the endpoint
-	Eigen::Isometry3f x_d;
-	Eigen::VectorXf xdot_d(6), xddot_d(6);
+	Eigen::Isometry3f x_d; Eigen::VectorXf xdot_d(6), xddot_d(6);
 	this->cartesianTrajectory.get_state(x_d, xdot_d, xddot_d, time);		// Desired state for give time
 
 	// Compute the mapping from Cartesian to joint space
 	Eigen::MatrixXf J = this->robot.get_jacobian();				// Get the Jacobian
-	Eigen::MatrixXf W = get_joint_limit_weighting(); // NOTE: THIS IS CURRENTLY THE INVERSE!
+	Eigen::MatrixXf W = get_joint_limit_weighting(); 				// NOTE: THIS IS CURRENTLY THE INVERSE!
 	Eigen::MatrixXf invWJt = W*J.transpose();					// Makes things a little faster
 	Eigen::MatrixXf invJ = invWJt*get_inverse(J*invWJt);				// Weighted pseudoinverse
 
 	// Compute the joint velocities to achieve the endpoint motion
 	Eigen::VectorXf qdot_R = invJ*(xdot_d + this->k*get_pose_error(x_d, this->robot.get_endpoint_pose())); // Range space vector
 	scale_velocity_vector(qdot_R, qdot_R);					// Ensure feasiblity of the range space vector
 
-// 	Eigen::MatrixXf N = Eigen::MatrixXf::Identity(this->n, this->n) - invJ*J;	// Null space projection matrix
-//	Eigen::VectorXf qdot_N = N*something;						// Null space vector
-
-// 	Eigen::VectorXf qdot = qdot_R + qdot_N;					// Combine the range and null space vectors
-//	scale_velocity_vector(qdot_N, qdot);						// Scale null space vector so joint velocities are in limits
-//	qdot_N = scale_velocity_vector(qdot_N, qdot);			
-//	qdot = qdot_R + qdot_N;
+	if(this->n <= 6) return qdot_R;						// No redundancy available
+	else										// Add a redundant task
+	{
+		Eigen::MatrixXf N = Eigen::MatrixXf::Identity(this->n, this->n) - invJ*J; // Null space projection matrix
+		Eigen::VectorXf qdot_N(this->n);					// Desired joint velocity
 
-	return qdot_R;
+//		NOTE TO SELF: Need to expand this later.
+//		switch(redunantTask)
+//		{
+//			case: manipulability
+//			{
+				qdot_N = N*W*optimise_manipulability(0.5, J);		// This is really N*inv(W)*qdot_d;
+//				break;
+//			}
+//			default:
+//			{
+//				¯\_(ツ)_/¯
+//				break;
+//			}
+//		}
+
+		Eigen::VectorXf qdot = qdot_R + qdot_N;				// Add the range space and null space vectors together
+		scale_velocity_vector(qdot_N, qdot);					// Scale null space vector to ensure feasibility
+		return qdot_R + qdot_N;						// Recombine after scaling
+	}
 }
 
 /******************** Get the inverse of a matrix, add damping as necessary ********************/
@@ -205,7 +221,7 @@ Eigen::MatrixXf SerialKinCtrl::get_inverse(const Eigen::MatrixXf &A)
 	Eigen::MatrixXf V = SVD.matrixV();						// V-matrix
 	Eigen::VectorXf s = SVD.singularValues();					// Get the singular values
 	Eigen::MatrixXf invA(A.cols(), A.rows()); invA.setZero();			// Value we want to return
-	
+
 	for(int i = 0; i < A.cols(); i++)
 	{
 		for(int j = 0; j < A.rows(); j++)
@@ -325,3 +341,32 @@ bool SerialKinCtrl::scale_velocity_vector(Eigen::VectorXf &vec, const Eigen::Vec
 		return true;
 	}
 }
+
+/******************** Get the gradient of manipulability to avoid singularities ********************/
+Eigen::VectorXf SerialKinCtrl::optimise_manipulability(const float &scalar, const Eigen::MatrixXf &J)
+{
+	// Variables used in this scope
+	Eigen::MatrixXf JJt = J*J.transpose();					// This makes things a little easier
+	Eigen::MatrixXf invJ = J.transpose()*get_inverse(JJt);			// Pseudoinverse of Jacobian
+	Eigen::MatrixXf dJ;								// Partial derivative of the Jacobian
+	Eigen::VectorXf grad(this->n);						// Value to be returned
+	float mu = sqrt(JJt.determinant());						// Actual measure of manipulability
+
+	if(scalar <= 0)
+	{
+		std::cerr << "[ERROR] [SERIALKINCTRL] optimise_manipulability () : Scalar " << scalar << " must be positive." << std::endl;
+		grad.setZero();							// Don't do anything!
+	}
+	else
+	{
+		grad[0] = 0.0;								// First joint doesn't affect manipulability
+
+		for(int i = 1; i < this->n; i++)
+		{
+			dJ = this->robot.get_partial_derivative(J, i);		// Get partial derivative w.r.t. ith joint
+			grad[i] = scalar*mu*(dJ*invJ).trace();			// Gradient of manipulability
+		}
+	}
+
+	return grad;
+}

Robot/Joint.h

@@ -0,0 +1,87 @@
+#ifndef JOINT_H_
+#define JOINT_H_
+
+#include <Eigen/Geometry>							// Eigen::Isometry3f, Eigen::Vector3f
+
+class Joint
+{
+	public:
+		Joint() {}							// Empty constructor
+
+		Joint(const Eigen::Isometry3f &origin,
+			const Eigen::Vector3f &axisOfActuation,
+			const float positionLimits[2],
+			const float &velocityLimit,
+			const float &torqueLimit,
+			const bool &revolute);
+
+		// Get Functions
+		bool is_revolute() const {return this->isRevolute;}		// Returns true if revolute
+		bool is_prismatic() const {return !this->isRevolute;}	// Returns true if NOT revolute
+		Eigen::Isometry3f get_pose(const float &pos);			// Get the pose from the displacement of this joint
+		Eigen::Vector3f get_axis() const {return this->axis;}
+		float get_velocity_limit() const {return this->vLim;}	// Get the speed limit
+		void get_position_limits(float &lower, float &upper);	// Get the position limits
+
+	private:
+		bool isRevolute = true;
+		Eigen::Isometry3f pose;					// Pose relative to some origin
+		Eigen::Vector3f axis = {0,0,1};				// Axis of actuation
+		float damping = 1.0;						// Viscous friction for the joint
+		float pLim[2] = {-M_PI, M_PI};				// Position limits
+		float vLim = 10;						// Speed limits
+		float tLim = 10;						// Torque / force limits
+
+};										// Semicolon needed after a class declaration
+
+/******************** Constructor ********************/
+Joint::Joint(const Eigen::Isometry3f &origin,
+		const Eigen::Vector3f &axisOfActuation,
+		const float positionLimits[2],
+		const float &velocityLimit,
+		const float &torqueLimit,
+		const bool &revolute)
+		: pose(origin)
+		, axis(axisOfActuation)
+		, pLim{positionLimits[0], positionLimits[1]}
+		, vLim(velocityLimit)
+		, tLim(torqueLimit)
+		, isRevolute(revolute)
+{
+	this->axis.normalize();						// Ensure unit norm
+
+	if(this->pLim[0] > this->pLim[1])
+	{
+		std::cerr << "[ERROR] [JOINT] Constructor : Lower joint limits of " << this->pLim[0]
+			<< " is greater than upper joint limit of " << this->pLim[1] << ". Swapping their values to avoid problems..." << std::endl;
+		float temp = this->pLim[1];
+		this->pLim[1] = this->pLim[0];
+		this->pLim[0] = temp;
+	}
+	if(this->vLim < 0)
+	{
+		std::cerr << "[ERROR] [JOINT] Constructor : Velocity limit " << this->vLim << " is less than zero!"
+			<< " Making the value positive to avoid problems..." << std::endl;
+		this->vLim *= -1;
+	}
+	if(this->vLim == 0) std::cerr << "[ERROR] [JOINT] Constructor : Velocity limit is zero!" << std::endl;
+}
+
+
+/******************** Get the transform from the joint displacement ********************/
+Eigen::Isometry3f Joint::get_pose(const float &pos)
+{
+	if(this->isRevolute) 	return this->pose*Eigen::AngleAxisf(pos, this->axis);
+	else			return this->pose*Eigen::Translation3f(pos*this->axis);
+}
+
+/******************** Get the position limits for the attached joint ********************/
+void Joint::get_position_limits(float &lower, float &upper)
+{
+	// Can't think of a good error check / message here ¯\_(ツ)_/¯
+	lower = this->pLim[0];
+	upper = this->pLim[1];
+}
+
+#endif
+