removed equating refs to positions and also unneeded couts

hsp-iit · Dec 17, 2024 · 5c0e3e0 · 5c0e3e0
1 parent 9b9aa25
commit 5c0e3e0
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 7 deletions.
diff --git a/src/BimanualControl.cpp b/src/BimanualControl.cpp
@@ -393,7 +393,7 @@ bool BimanualControl::move_to_poses(const std::vector<Eigen::Isometry3d> &left,
 bool BimanualControl::move_object(const Eigen::Isometry3d &pose,
                                   const double &time)
 {
-	std::cout<<"\n Move Object called";
+
 	if(not this->isGrasping)
 	{
 		std::cerr << "[ERROR] [BIMANUAL CONTROL] move_object(): "
@@ -421,13 +421,13 @@ bool BimanualControl::move_object(const Eigen::Isometry3d &pose,
 
 			std::vector<double> times;
 			times.push_back(time);
-			std::cout<<"Setting initial and Previous Object Pose)";
+
 
 		return move_object(poses,times);
 		}
 
 		else 
-		{	std::cout <<"\n New pose given \n"<< pose.translation();
+		{	
 			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)
 			{
@@ -463,7 +463,7 @@ bool BimanualControl::move_object(const std::vector<Eigen::Isometry3d> &poses,
 
 		return false;
 	}
-	std::cout<<"Moving object through poses";
+
 	Eigen::Vector<double,6> leftHandTwist = iDynTree::toEigen(this->computer.getFrameVel("left"));
 	Eigen::Vector3d angularVel = leftHandTwist.tail(3);
 
@@ -650,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()
 	}
@@ -874,7 +874,7 @@ void BimanualControl::run()
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 void BimanualControl::threadRelease()
 {
-	this->jointRef = this->jointPos;
+	//this->jointRef = this->jointPos;
 }
 
   ////////////////////////////////////////////////////////////////////////////////////////////////////

diff --git a/src/ControlServer.cpp b/src/ControlServer.cpp
@@ -184,7 +184,6 @@ class ControlServer : public ControlInterface
 			if(custom_continuous_action)
 			{
 				std::string reset="reset";
-				std::cout<<"\n Sending custom action \n";
 				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()));