From 07bce7d6637493f4d4211263bb015c2ca8d0b75b Mon Sep 17 00:00:00 2001 From: ergocub Date: Mon, 8 Jul 2024 10:40:06 +0200 Subject: [PATCH 01/10] last uncommitted change on real robot --- config/real_robot/joint_space_actions_wristless.ini | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/config/real_robot/joint_space_actions_wristless.ini b/config/real_robot/joint_space_actions_wristless.ini index e9042a6..73549b5 100644 --- a/config/real_robot/joint_space_actions_wristless.ini +++ b/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) ) From 1abe78bda77c880b5623b5b9445327a635a41cad Mon Sep 17 00:00:00 2001 From: ergocub Date: Thu, 18 Jul 2024 18:49:26 +0200 Subject: [PATCH 02/10] some minor changes to ensure any original configs are not changed and faster actions --- src/ControlServer.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/ControlServer.cpp b/src/ControlServer.cpp index 99a2f44..89091ee 100644 --- a/src/ControlServer.cpp +++ b/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("reset"); //Temporary holde rof the reset pose values. bool custom_continuous_action = false; std::vector object_pose; - + CartesianMotion reset_custom; if(temp == graspActionMap->end()) { std::stringstream ss(actionName); @@ -185,17 +185,20 @@ class ControlServer : public ControlInterface { 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; - + CartesianMotion modify_reset = temp2->second; + modify_reset.waypoints[0].translate(Eigen::Vector3d(object_pose[0],object_pose[1],object_pose[2])); + modify_reset.waypoints[0].rotate(Eigen::AngleAxisd(object_pose[3],Eigen::Vector3d::UnitX())); + reset_custom.waypoints = temp ->second.waypoints; + temp ->second.waypoints = modify_reset.waypoints; + } objectWaypoints = temp->second.waypoints; waypointTimes = temp->second.times; type = temp->second.type; + if(custom_continuous_action) + temp->second.waypoints = reset_custom.waypoints; + if(type == absolute) { return this->robot->move_object(objectWaypoints,waypointTimes); From 3897c4b26e2970829cd11920eb4936fd07b31fc9 Mon Sep 17 00:00:00 2001 From: Vignesh Sushrutha Raghavan Date: Thu, 29 Aug 2024 13:15:59 +0200 Subject: [PATCH 03/10] Execute relative object pose changes through custom command instead of absolute --- config/real_robot/grasp_actions.ini | 2 +- src/ControlServer.cpp | 19 ++++++++++++++----- 2 files changed, 15 insertions(+), 6 deletions(-) diff --git a/config/real_robot/grasp_actions.ini b/config/real_robot/grasp_actions.ini index 71be8c1..c8d2b94 100644 --- a/config/real_robot/grasp_actions.ini +++ b/config/real_robot/grasp_actions.ini @@ -161,7 +161,7 @@ custom ( points ((0.0 0.0 0.00\ times (3.0) - type absolute) + type relative) reset ( points ((0.35 0.00 0.20\ 0.00 0.00 0.00)) diff --git a/src/ControlServer.cpp b/src/ControlServer.cpp index 89091ee..5e37810 100644 --- a/src/ControlServer.cpp +++ b/src/ControlServer.cpp @@ -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 '" @@ -185,19 +185,28 @@ class ControlServer : public ControlInterface { std::string reset="reset"; temp = graspActionMap->find("custom"); - CartesianMotion modify_reset = temp2->second; + CartesianMotion modify_reset = temp->second; + reset_custom = temp->second; modify_reset.waypoints[0].translate(Eigen::Vector3d(object_pose[0],object_pose[1],object_pose[2])); modify_reset.waypoints[0].rotate(Eigen::AngleAxisd(object_pose[3],Eigen::Vector3d::UnitX())); - reset_custom.waypoints = temp ->second.waypoints; - temp ->second.waypoints = modify_reset.waypoints; + + temp->second.waypoints.clear(); + temp->second.times.clear(); + temp->second.waypoints.push_back(modify_reset.waypoints[0]); + temp->second.times.push_back( object_pose[4]); + } objectWaypoints = temp->second.waypoints; waypointTimes = temp->second.times; type = temp->second.type; if(custom_continuous_action) - temp->second.waypoints = reset_custom.waypoints; + { + temp->second.waypoints.clear(); + temp->second.times.clear(); + temp->second = reset_custom; + } if(type == absolute) { From 01bfea08ef4bb0f9f58e07123be8376fa960c713 Mon Sep 17 00:00:00 2001 From: vigisushrutha23 Date: Fri, 30 Aug 2024 16:09:35 +0000 Subject: [PATCH 04/10] adding an initial object pose to reduce drift when reset is called --- include/BimanualControl.h | 10 ++++++++++ src/BimanualControl.cpp | 2 ++ src/CommandPrompt.cpp | 2 +- src/ControlServer.cpp | 5 +++++ 4 files changed, 18 insertions(+), 1 deletion(-) diff --git a/include/BimanualControl.h b/include/BimanualControl.h index ab83e69..6921b1e 100644 --- a/include/BimanualControl.h +++ b/include/BimanualControl.h @@ -187,6 +187,14 @@ class BimanualControl : public QPSolver, */ 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,6 +280,8 @@ class BimanualControl : public QPSolver, 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. /** * Specifies joint control mode, or Cartesian control mode. diff --git a/src/BimanualControl.cpp b/src/BimanualControl.cpp index 3c21a5b..4d9a4b2 100644 --- a/src/BimanualControl.cpp +++ b/src/BimanualControl.cpp @@ -880,6 +880,8 @@ bool BimanualControl::activate_grasp() 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 + + this->initGraspObjectPose = this->objectPose; //Pose to reset to to avoid drift. return move_object(this->objectPose,1.0); // Hold object in current pose } diff --git a/src/CommandPrompt.cpp b/src/CommandPrompt.cpp index 9d317e1..23dee8d 100644 --- a/src/CommandPrompt.cpp +++ b/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() >= 4) output.addString("Cosa"); else { diff --git a/src/ControlServer.cpp b/src/ControlServer.cpp index 5e37810..6eaf4e0 100644 --- a/src/ControlServer.cpp +++ b/src/ControlServer.cpp @@ -210,6 +210,11 @@ class ControlServer : public ControlInterface if(type == absolute) { + if(actionName == "reset") + { + objectWaypoints.clear(); + objectWaypoints.push_back(this->robot->getInitialGraspObjectPose()); + } return this->robot->move_object(objectWaypoints,waypointTimes); } else // type == relative From d738d5cf47da658ba5705721cfe6f362ce12c4bc Mon Sep 17 00:00:00 2001 From: ergocub Date: Mon, 2 Sep 2024 13:41:19 +0200 Subject: [PATCH 05/10] minor bug fixes --- config/real_robot/grasp_actions.ini | 2 +- src/CommandPrompt.cpp | 2 +- src/ControlServer.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/config/real_robot/grasp_actions.ini b/config/real_robot/grasp_actions.ini index c8d2b94..71be8c1 100644 --- a/config/real_robot/grasp_actions.ini +++ b/config/real_robot/grasp_actions.ini @@ -161,7 +161,7 @@ custom ( points ((0.0 0.0 0.00\ times (3.0) - type relative) + type absolute) reset ( points ((0.35 0.00 0.20\ 0.00 0.00 0.00)) diff --git a/src/CommandPrompt.cpp b/src/CommandPrompt.cpp index 23dee8d..61de316 100644 --- a/src/CommandPrompt.cpp +++ b/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 { diff --git a/src/ControlServer.cpp b/src/ControlServer.cpp index 6eaf4e0..6c9a344 100644 --- a/src/ControlServer.cpp +++ b/src/ControlServer.cpp @@ -185,7 +185,7 @@ class ControlServer : public ControlInterface { std::string reset="reset"; temp = graspActionMap->find("custom"); - CartesianMotion modify_reset = temp->second; + CartesianMotion modify_reset = temp2->second; reset_custom = temp->second; modify_reset.waypoints[0].translate(Eigen::Vector3d(object_pose[0],object_pose[1],object_pose[2])); modify_reset.waypoints[0].rotate(Eigen::AngleAxisd(object_pose[3],Eigen::Vector3d::UnitX())); From 784c738bb903a2e6a1279b22b9033f28f96ca068 Mon Sep 17 00:00:00 2001 From: Vignesh Sushrutha Raghavan Date: Fri, 18 Oct 2024 14:53:21 +0000 Subject: [PATCH 06/10] Changed grasp pose initial assignment to include full translation --- src/BimanualControl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/BimanualControl.cpp b/src/BimanualControl.cpp index 4d9a4b2..c3286ef 100644 --- a/src/BimanualControl.cpp +++ b/src/BimanualControl.cpp @@ -875,9 +875,9 @@ bool BimanualControl::activate_grasp() { this->isGrasping = true; // Set grasp constraint - double graspWidth = (this->leftPose.translation() - this->rightPose.translation()).norm(); // Distance between the hands + Eigen::VectorXd graspWidth = (this->leftPose.translation() - this->rightPose.translation()); // Distance between the hands - this->leftHand2Object = Eigen::Isometry3d(Eigen::Translation3d(0,-graspWidth/2,0)); // Assume object is rigidly attached to left hand + this->leftHand2Object = Eigen::Isometry3d(Eigen::Translation3d(graspWidth(0),-graspWidth(1)/2,graspWidth(2))); // Assume object is rigidly attached to left hand this->objectPose = this->leftPose*this->leftHand2Object; // Update the object pose From 3ada34aaef6f469f5dcf33c3eb74779eb65b82d5 Mon Sep 17 00:00:00 2001 From: Vignesh Sushrutha Raghavan Date: Tue, 22 Oct 2024 13:31:09 +0000 Subject: [PATCH 07/10] further adjustments to help with grasp motion accuracy. To be tested --- config/gazebo_sim/cartesian_actions.ini | 10 ++++----- include/BimanualControl.h | 3 +++ src/BimanualControl.cpp | 27 ++++++++++++++++++------- src/ControlServer.cpp | 4 ++-- 4 files changed, 30 insertions(+), 14 deletions(-) diff --git a/config/gazebo_sim/cartesian_actions.ini b/config/gazebo_sim/cartesian_actions.ini index fbb9e03..6c75c00 100644 --- a/config/gazebo_sim/cartesian_actions.ini +++ b/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 ) ) diff --git a/include/BimanualControl.h b/include/BimanualControl.h index 6921b1e..c8f9fbd 100644 --- a/include/BimanualControl.h +++ b/include/BimanualControl.h @@ -282,6 +282,9 @@ class BimanualControl : public QPSolver, 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. + /** * Specifies joint control mode, or Cartesian control mode. diff --git a/src/BimanualControl.cpp b/src/BimanualControl.cpp index c3286ef..36a0955 100644 --- a/src/BimanualControl.cpp +++ b/src/BimanualControl.cpp @@ -135,7 +135,15 @@ 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.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 ] @@ -875,15 +883,20 @@ bool BimanualControl::activate_grasp() { this->isGrasping = true; // Set grasp constraint - Eigen::VectorXd graspWidth = (this->leftPose.translation() - this->rightPose.translation()); // Distance between the hands - - this->leftHand2Object = Eigen::Isometry3d(Eigen::Translation3d(graspWidth(0),-graspWidth(1)/2,graspWidth(2))); // 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. + 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,1.0); // Hold object in current pose } } diff --git a/src/ControlServer.cpp b/src/ControlServer.cpp index 6c9a344..36cfa60 100644 --- a/src/ControlServer.cpp +++ b/src/ControlServer.cpp @@ -152,7 +152,7 @@ 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 object_pose; @@ -193,7 +193,7 @@ class ControlServer : public ControlInterface temp->second.waypoints.clear(); temp->second.times.clear(); - temp->second.waypoints.push_back(modify_reset.waypoints[0]); + temp->second.waypoints.push_back(this->robot->getInitialGraspObjectPose()*modify_reset.waypoints[0]); temp->second.times.push_back( object_pose[4]); } From 56c2eacf2eb263654b66a4c131f7a709b83222d6 Mon Sep 17 00:00:00 2001 From: Vignesh Sushrutha Raghavan Date: Fri, 25 Oct 2024 12:15:15 +0200 Subject: [PATCH 08/10] minor changes. The left seems to follow right hand in all axes. needs to be plotted with ros --- src/BimanualControl.cpp | 6 ++++++ src/ControlServer.cpp | 21 ++++++++++++--------- 2 files changed, 18 insertions(+), 9 deletions(-) diff --git a/src/BimanualControl.cpp b/src/BimanualControl.cpp index 36a0955..a67a793 100644 --- a/src/BimanualControl.cpp +++ b/src/BimanualControl.cpp @@ -140,9 +140,15 @@ bool BimanualControl::update_state() 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 + + std::cout<<"\n Current Object pose Rotation\n" <objectPose.linear().eulerAngles(0,1,2) <<"\n Translation \n"<objectPose.translation(); // G = [ I 0 I 0 ] diff --git a/src/ControlServer.cpp b/src/ControlServer.cpp index 36cfa60..c9cba3f 100644 --- a/src/ControlServer.cpp +++ b/src/ControlServer.cpp @@ -186,14 +186,15 @@ class ControlServer : public ControlInterface std::string reset="reset"; temp = graspActionMap->find("custom"); CartesianMotion modify_reset = temp2->second; + Eigen::Isometry3d init_pose_to_new = this->robot->getInitialGraspObjectPose(); reset_custom = temp->second; - modify_reset.waypoints[0].translate(Eigen::Vector3d(object_pose[0],object_pose[1],object_pose[2])); - modify_reset.waypoints[0].rotate(Eigen::AngleAxisd(object_pose[3],Eigen::Vector3d::UnitX())); + 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())); temp->second.waypoints.clear(); temp->second.times.clear(); - temp->second.waypoints.push_back(this->robot->getInitialGraspObjectPose()*modify_reset.waypoints[0]); + temp->second.waypoints.push_back(init_pose_to_new); temp->second.times.push_back( object_pose[4]); } @@ -201,12 +202,7 @@ class ControlServer : public ControlInterface waypointTimes = temp->second.times; type = temp->second.type; - if(custom_continuous_action) - { - temp->second.waypoints.clear(); - temp->second.times.clear(); - temp->second = reset_custom; - } + if(type == absolute) { @@ -231,6 +227,13 @@ class ControlServer : public ControlInterface } return this->robot->move_object(newObjectPoints,waypointTimes); } + + if(custom_continuous_action) + { + temp->second.waypoints.clear(); + temp->second.times.clear(); + temp->second = reset_custom; + } } /** From 9b9aa25a4d5fc85bca30e0f592d31d9fb69267c9 Mon Sep 17 00:00:00 2001 From: Vignesh Sushrutha Raghavan Date: Fri, 8 Nov 2024 17:59:48 +0100 Subject: [PATCH 09/10] changes to traj generation to ensure lower drift and smoother operation --- config/real_robot/cartesian_actions.ini | 13 ++++- include/BimanualControl.h | 6 +++ src/BimanualControl.cpp | 64 ++++++++++++++++++++----- src/CartesianTrajectory.cpp | 5 +- src/ControlServer.cpp | 22 ++------- 5 files changed, 79 insertions(+), 31 deletions(-) diff --git a/config/real_robot/cartesian_actions.ini b/config/real_robot/cartesian_actions.ini index c37ed72..4a9912b 100644 --- a/config/real_robot/cartesian_actions.ini +++ b/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) + ) diff --git a/include/BimanualControl.h b/include/BimanualControl.h index c8f9fbd..3fd02b3 100644 --- a/include/BimanualControl.h +++ b/include/BimanualControl.h @@ -285,7 +285,13 @@ class BimanualControl : public QPSolver, 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. */ diff --git a/src/BimanualControl.cpp b/src/BimanualControl.cpp index a67a793..651b860 100644 --- a/src/BimanualControl.cpp +++ b/src/BimanualControl.cpp @@ -148,7 +148,6 @@ bool BimanualControl::update_state() this->objectPose = this->leftPose*this->leftHand2Object; // Assume object is rigidly attached to left hand - std::cout<<"\n Current Object pose Rotation\n" <objectPose.linear().eulerAngles(0,1,2) <<"\n Translation \n"<objectPose.translation(); // G = [ I 0 I 0 ] @@ -394,6 +393,7 @@ bool BimanualControl::move_to_poses(const std::vector &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(): " @@ -411,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 poses; - poses.push_back(pose); + if(!this->previousObjectPoseSet) + { + this->previousObjectPoseSet = true; + this->previousObjectPose = pose; + + std::vector poses; + poses.push_back(pose); - std::vector times; - times.push_back(time); + std::vector times; + times.push_back(time); + std::cout<<"Setting initial and Previous Object Pose)"; - return move_object(poses,times); // Pass onward for spline generation + return move_object(poses,times); + } + + else + { std::cout <<"\n New pose given \n"<< pose.translation(); + Eigen::Matrix 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 poses; + poses.push_back(pose); + + std::vector times; + times.push_back(time); + + return move_object(poses,times); + } + else + return true; + } + + // Pass onward for spline generation } } @@ -434,15 +463,22 @@ bool BimanualControl::move_object(const std::vector &poses, return false; } - + std::cout<<"Moving object through poses"; Eigen::Vector leftHandTwist = iDynTree::toEigen(this->computer.getFrameVel("left")); Eigen::Vector3d angularVel = leftHandTwist.tail(3); Eigen::Vector 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 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 @@ -451,7 +487,11 @@ bool BimanualControl::move_object(const std::vector &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 waypoints; waypoints.push_back(this->objectPose); // First waypoint is current pose + std::vector 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 @@ -902,7 +942,7 @@ bool BimanualControl::activate_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 } } @@ -914,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 diff --git a/src/CartesianTrajectory.cpp b/src/CartesianTrajectory.cpp index 610e55a..962ffd7 100644 --- a/src/CartesianTrajectory.cpp +++ b/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]); diff --git a/src/ControlServer.cpp b/src/ControlServer.cpp index c9cba3f..794841d 100644 --- a/src/ControlServer.cpp +++ b/src/ControlServer.cpp @@ -184,19 +184,12 @@ class ControlServer : public ControlInterface if(custom_continuous_action) { std::string reset="reset"; - temp = graspActionMap->find("custom"); - CartesianMotion modify_reset = temp2->second; + std::cout<<"\n Sending custom action \n"; Eigen::Isometry3d init_pose_to_new = this->robot->getInitialGraspObjectPose(); - reset_custom = temp->second; 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); - temp->second.waypoints.clear(); - temp->second.times.clear(); - temp->second.waypoints.push_back(init_pose_to_new); - temp->second.times.push_back( object_pose[4]); - } objectWaypoints = temp->second.waypoints; waypointTimes = temp->second.times; @@ -209,9 +202,9 @@ class ControlServer : public ControlInterface if(actionName == "reset") { objectWaypoints.clear(); - objectWaypoints.push_back(this->robot->getInitialGraspObjectPose()); + return this->robot->move_object(this->robot->getInitialGraspObjectPose(),waypointTimes[0]); } - return this->robot->move_object(objectWaypoints,waypointTimes); + } else // type == relative { @@ -228,12 +221,7 @@ class ControlServer : public ControlInterface return this->robot->move_object(newObjectPoints,waypointTimes); } - if(custom_continuous_action) - { - temp->second.waypoints.clear(); - temp->second.times.clear(); - temp->second = reset_custom; - } + } /** From 5c0e3e0dd7adf0455966573af9ece5a18cda0fe2 Mon Sep 17 00:00:00 2001 From: Vignesh Sushrutha Raghavan Date: Tue, 17 Dec 2024 17:06:30 +0100 Subject: [PATCH 10/10] removed equating refs to positions and also unneeded couts --- src/BimanualControl.cpp | 12 ++++++------ src/ControlServer.cpp | 1 - 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/src/BimanualControl.cpp b/src/BimanualControl.cpp index 651b860..81f3631 100644 --- a/src/BimanualControl.cpp +++ b/src/BimanualControl.cpp @@ -393,7 +393,7 @@ bool BimanualControl::move_to_poses(const std::vector &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 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 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 &poses, return false; } - std::cout<<"Moving object through poses"; + Eigen::Vector 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 index 794841d..abb9d06 100644 --- 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()));