Merge pull request #7 from hsp-iit/all_changes_for_nav_stack

All changes to work with navigation module and reduction in drift

config/gazebo_sim/cartesian_actions.ini

@@ -49,10 +49,10 @@ left ( up ( points ((0.0, 0.0, 0.05 \
    	       times (2.0)
    	       type relative)
 
-        testgrasp ( points ( (0.3, 0.32, 0.2\
+        testgrasp ( points ( (0.30, 0.32, 0.2\
                               0.0, 0.0, 0.0)
 
-                             (0.3, 0.28, 0.2\
+                             (0.30, 0.28, 0.2\
                               0.0, 0.0, 0.0))
 
                 times (4.0, 6.0)
@@ -101,13 +101,13 @@ right ( up ( points ((0.0, 0.0, 0.05 \
    	       times (2.0)
    	       type relative)
 
-        testgrasp ( points ((0.35, -0.18, 0.2\
+        testgrasp ( points ((0.30, -0.32, 0.2\
                              0.00,  0.00, 0.0) 
 
-                            (0.35, -0.15, 0.2\
+                            (0.30, -0.28, 0.2\
                              0.00,  0.00, 0.0))
 
-                times (3.0 4.0)
+                times (4.0 6.0)
 
                 type absolute )
       )

config/real_robot/cartesian_actions.ini

@@ -7,7 +7,7 @@
 
 [CARTESIAN_ACTIONS]
 
-names (up down left right in out forward back testgrasp)  # NEED TO DECLARE THE NAMES SO THEY CAN BE SEARCHED
+names (up down left right in out forward back testgrasp right_forward)  # NEED TO DECLARE THE NAMES SO THEY CAN BE SEARCHED
 
 left ( up ( points ((0.0, 0.0, 0.05 \
                      0.0, 0.0, 0.00))
@@ -58,6 +58,11 @@ left ( up ( points ((0.0, 0.0, 0.05 \
                 times (4.0, 6.0)
 
                 type absolute )
+
+       right_forward ( points ((0.3, 0.28, 0.2\
+                              0.0, 0.0, 0.0))
+   	       times (5.0)
+   	       type absolute)
 
      )
 
@@ -110,4 +115,10 @@ right ( up ( points ((0.0, 0.0, 0.05 \
                 times (4.0 6.0)
 
                 type absolute )
+
+       right_forward ( points ((0.65, -0.26, 0.31 \
+   	                 0.00, 0.0, 0.0))
+   	       times (5.0)
+   	       type absolute)
+
       )

config/real_robot/joint_space_actions_wristless.ini

@@ -19,8 +19,8 @@ ready ( points ((0.0  0.0  0.0 \
         times (1.5) )
 
 bottom_grasp ( points ((0.0  0.0  0.0 \
-                       -0.31  0.0  -0.05  1.24   0.0   0.0   0.0 \
-                       -0.31  0.0  -0.05  1.24   0.0   0.0   0.0 ))
+                       -0.31  0.0  -0.02  1.24   0.0   0.0   0.0 \
+                       -0.31  0.0  -0.02  1.24   0.0   0.0   0.0 ))
 
         times (1.5) )
 

include/BimanualControl.h

@@ -187,6 +187,14 @@ class BimanualControl : public QPSolver<double>,
 		  */
 		 void motor_control(const bool &active);
 
+		/**
+		 * @brief Get the Initial Grasp Object Pose To Reduce Drift
+		 * 
+		 * @return Initial pose of the grasped object 
+		 * 
+		 */
+		inline Eigen::Isometry3d getInitialGraspObjectPose(){return initGraspObjectPose;}
+
 	private:
 		bool isGrasping = false;                                                            ///< Used to check which control method to use
 
@@ -272,7 +280,18 @@ class BimanualControl : public QPSolver<double>,
 		Eigen::Isometry3d objectPose;                                                       ///< Pose of the object
 
 		Eigen::Isometry3d leftHand2Object;                                                  ///< Pose of the object relative to left hand
+
+		Eigen::Isometry3d initGraspObjectPose;												///< Initial pose to move the object to when reset command has be called.
+
+		Eigen::Isometry3d initObjectRelPose;												///< Initial pose of object relative to the left had.
+
+		Eigen::Isometry3d previousObjectPose;													///< Previous pose of the object.
 
+		bool previousObjectPoseSet = false;
+
+
+
+
 		/**
 		 * Specifies joint control mode, or Cartesian control mode.
 		 */

src/BimanualControl.cpp

@@ -135,7 +135,20 @@ bool BimanualControl::update_state()
 
 			if(this->isGrasping)
 			{
+				Eigen::Matrix4d leftToRightMat = this->rightPose.matrix()*this->leftPose.matrix().inverse(); 
+
+
+				this->leftHand2Object = Eigen::Isometry3d(Eigen::Translation3d(this->initObjectRelPose.translation())); //Apply intially recorded grasp constraint
+
+				//this->leftHand2Object.matrix() = leftToRightMat;
+
+				//this->leftHand2Object.matrix()(1,3) *= 0.5;
+
+				this->leftHand2Object.linear() = leftToRightMat.block(0,0,3,3); //Get the actual orientation based on current rel pose of right hand w.r.t left.
+
 				this->objectPose = this->leftPose*this->leftHand2Object;            // Assume object is rigidly attached to left hand
+
+
 
 				// G = [    I    0     I    0 ]
 				//     [ S(left) I S(right) I ]
@@ -380,6 +393,7 @@ bool BimanualControl::move_to_poses(const std::vector<Eigen::Isometry3d> &left,
 bool BimanualControl::move_object(const Eigen::Isometry3d &pose,
                                   const double &time)
 {
+
 	if(not this->isGrasping)
 	{
 		std::cerr << "[ERROR] [BIMANUAL CONTROL] move_object(): "
@@ -397,13 +411,42 @@ bool BimanualControl::move_object(const Eigen::Isometry3d &pose,
 	else
 	{
 		// Insert in to std::vector objects and pass on to spline generator
-		std::vector<Eigen::Isometry3d> poses;
-		poses.push_back(pose);
+		if(!this->previousObjectPoseSet)
+		{
+			this->previousObjectPoseSet = true;
+			this->previousObjectPose = pose;
+
+			std::vector<Eigen::Isometry3d> poses;
+			poses.push_back(pose);
 
-		std::vector<double> times;
-		times.push_back(time);
+			std::vector<double> times;
+			times.push_back(time);
+
 
-		return move_object(poses,times);                                                    // Pass onward for spline generation
+		return move_object(poses,times);
+		}
+
+		else 
+		{	
+			Eigen::Matrix<double,6,1> change_in_pose = pose_error(pose,this->previousObjectPose);
+			if(change_in_pose.block(0,0,3,1).norm()>0.02 || change_in_pose.block(3,0,3,1).norm()>0.1)
+			{
+
+				this->previousObjectPose = pose;
+
+				std::vector<Eigen::Isometry3d> poses;
+				poses.push_back(pose);
+
+				std::vector<double> times;
+				times.push_back(time);
+
+				return move_object(poses,times);
+			}
+			else 
+				return true;
+		}
+
+		                                                    // Pass onward for spline generation
 	}
 }
 
@@ -420,15 +463,22 @@ bool BimanualControl::move_object(const std::vector<Eigen::Isometry3d> &poses,
 
 		return false;
 	}
-	
+
 	Eigen::Vector<double,6> leftHandTwist = iDynTree::toEigen(this->computer.getFrameVel("left"));
 	Eigen::Vector3d angularVel = leftHandTwist.tail(3);
 
 	Eigen::Vector<double,6> objectTwist;
 	objectTwist.head(3) = leftHandTwist.head(3) + angularVel.cross(this->objectPose.translation() - this->leftPose.translation());
 	objectTwist.tail(3) = angularVel;                                  
-
-	if( isRunning() ) stop();                                                                   // Stop any control threads that are running
+	Eigen::Isometry3d pose;
+	Eigen::Vector<double,6> vel, acc;
+	double elapsedTime = yarp::os::Time::now() - this->startTime;
+	bool manip_in_progress = false;
+	if( isRunning() ){
+		if(this->objectTrajectory.get_state(pose,vel,acc,elapsedTime+0.03))
+			manip_in_progress = true;
+		 stop();
+	}                                                                   // Stop any control threads that are running
 
 	this->controlSpace = cartesian;                                                             // Ensure that we are running in Cartesian mode
 
@@ -437,7 +487,11 @@ bool BimanualControl::move_object(const std::vector<Eigen::Isometry3d> &poses,
 	t.insert(t.end(),times.begin(),times.end());                                                // Add on the rest of the times
 
 	// Set up the waypoints for the object
-	std::vector<Eigen::Isometry3d> waypoints; waypoints.push_back(this->objectPose);            // First waypoint is current pose
+	std::vector<Eigen::Isometry3d> waypoints; 
+	if(manip_in_progress)
+		waypoints.push_back(pose);            // First waypoint is current pose
+	else 
+		waypoints.push_back(this->objectPose);            // First waypoint is current pose
 	waypoints.insert(waypoints.end(),poses.begin(),poses.end());                                // Add on additional waypoints
 
 	try
@@ -596,7 +650,7 @@ bool BimanualControl::threadInit()
 
 		this->startTime = yarp::os::Time::now();                                            // Used to time the control loop
 
-		this->jointRef = this->jointPos;                                                    // Reference position = current position
+		//this->jointRef = this->jointPos;                                                    // Reference position = current position
 
 		return true;                                                                        // jumps immediately to run()
 	}
@@ -820,7 +874,7 @@ void BimanualControl::run()
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 void BimanualControl::threadRelease()
 {
-	this->jointRef = this->jointPos;
+	//this->jointRef = this->jointPos;
 }
 
   ////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -875,13 +929,20 @@ bool BimanualControl::activate_grasp()
 	{		
 		this->isGrasping = true;                                                            // Set grasp constraint
 
-		double graspWidth = (this->leftPose.translation() - this->rightPose.translation()).norm(); // Distance between the hands
-
-		this->leftHand2Object = Eigen::Isometry3d(Eigen::Translation3d(0,-graspWidth/2,0)); // Assume object is rigidly attached to left hand
-
-		this->objectPose = this->leftPose*this->leftHand2Object;                            // Update the object pose
+		Eigen::Matrix4d leftToRightMat = this->rightPose.matrix()*this->leftPose.matrix().inverse(); 
+
+		Eigen::Matrix4d leftToObjectMat = leftToRightMat;
+		leftToObjectMat(1,3) = 0.5*leftToRightMat(1,3); 			// modifying it to match the frame on the object
+
+		this->leftHand2Object.matrix() = leftToObjectMat;
+
+		this->objectPose = this->leftHand2Object*this->leftPose;    // Update the object pose
+
+		this->initObjectRelPose.matrix() = leftToObjectMat;			//Store relative pose to match initial rel pose in the hope that it constrains the grasp
+
+		this->initGraspObjectPose = this->objectPose;				//Pose to reset to to avoid drift.
 
-		return move_object(this->objectPose,1.0);                                           // Hold object in current pose
+		return move_object(this->objectPose,3.0);                   // Hold object in current pose
 	}
 }
 
@@ -893,7 +954,7 @@ bool BimanualControl::release_grasp()
 	if(this->isGrasping)
 	{
 		this->isGrasping = false;
-
+		this->previousObjectPoseSet = false;
 		return move_to_pose(this->leftPose,this->rightPose,1.0);
 	}
 	else

src/CartesianTrajectory.cpp

@@ -111,7 +111,10 @@ bool CartesianTrajectory::get_state(Eigen::Isometry3d         &pose,
 {
 	double pos[3];                                                                              // Position vector
 	double rot[3];                                                                              // Angle*axis
-
+	if(spline.size()==0)
+	{
+		return false;
+	}
 	for(int i = 0; i < 3; i++)
 	{
 		pos[i] = this->spline[ i ].evaluatePoint(time, vel[i]  , acc[i]);

src/CommandPrompt.cpp

@@ -126,7 +126,7 @@ int main(int argc, char *argv[])
 					if (ss.peek() == ',')
 						ss.ignore();
 				}
-				if(object_pose.size() <= 2 && object_pose.size() >= 4)
+				if(object_pose.size() <= 2 || object_pose.size() > 5)
 					output.addString("Cosa");
 				else
 				{

src/ControlServer.cpp

@@ -152,11 +152,11 @@ class ControlServer : public ControlInterface
 				return false;
 			}
 			auto temp = graspActionMap->find(actionName);                               // Temporary placeholder for the iterator
-			auto temp2 = *graspActionMap->find("reset");									//Temporary holde rof the reset pose values.
+			auto temp2 = graspActionMap->find("custom");									//Temporary holde rof the reset pose values.
 
 			bool custom_continuous_action = false;
 			std::vector<double> object_pose;
-
+                        CartesianMotion reset_custom;
 			if(temp == graspActionMap->end())
 			{
 				std::stringstream ss(actionName);
@@ -168,7 +168,7 @@ class ControlServer : public ControlInterface
         				ss.ignore();
 
 				}
-				if(object_pose.size()<=2 && object_pose.size()>=4)
+				if(object_pose.size()<=2 || object_pose.size()>5)
 				{
 					std::cerr << "[ERROR] [CONTROL SERVER} perform_grasp_action(): "
 				          	<< "Could not find the action named '"
@@ -184,21 +184,26 @@ class ControlServer : public ControlInterface
 			if(custom_continuous_action)
 			{
 				std::string reset="reset";
-				temp = graspActionMap->find("custom");
-				temp2.second.waypoints[0].translate(Eigen::Vector3d(object_pose[0],object_pose[1],object_pose[2]));
-				temp2.second.waypoints[0].rotate(Eigen::AngleAxisd(object_pose[3],Eigen::Vector3d::UnitX()));
-				temp ->second.waypoints = temp2.second.waypoints;
-				temp ->second.times = temp2.second.times;
-				temp ->second.type = temp2.second.type;
+				Eigen::Isometry3d init_pose_to_new = this->robot->getInitialGraspObjectPose();
+				init_pose_to_new.translate(Eigen::Vector3d(object_pose[0],object_pose[1],object_pose[2]));
+				init_pose_to_new.rotate(Eigen::AngleAxisd(object_pose[3],Eigen::Vector3d::UnitX()));
+				return this->robot->move_object(init_pose_to_new,3.0);
 
 			}
 			objectWaypoints = temp->second.waypoints;
 			waypointTimes   = temp->second.times;
 			type            = temp->second.type;
 
+
+
 			if(type == absolute)
 			{
-				return this->robot->move_object(objectWaypoints,waypointTimes);
+				if(actionName == "reset")
+				{
+					objectWaypoints.clear();
+					return this->robot->move_object(this->robot->getInitialGraspObjectPose(),waypointTimes[0]);
+				}
+
 			}
 			else // type == relative
 			{
@@ -214,6 +219,8 @@ class ControlServer : public ControlInterface
 				}
 				return this->robot->move_object(newObjectPoints,waypointTimes);
 			}
+
+
 		}
 
 		/**