From 7a45ea2369bdc2233ebc95f3438b053a46048867 Mon Sep 17 00:00:00 2001 From: Francesco Riccio Date: Mon, 2 Dec 2013 14:16:58 +0100 Subject: [PATCH 1/6] diveType #define --- machineLearning/GoalieLearner.h | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/machineLearning/GoalieLearner.h b/machineLearning/GoalieLearner.h index 2d3a068..b331bc9 100644 --- a/machineLearning/GoalieLearner.h +++ b/machineLearning/GoalieLearner.h @@ -4,6 +4,14 @@ // Uncomment if you want to have debug information #define GOALIE_DEBUG_MODE +#define DIVE_TYPE(x) \ + if(x == 1) std::cerr << "\033[22;34;1m"<<"\t[Goalie] diveType: none"<<"\033[0m" << std::endl; \ + else if(x == 2) std::cerr << "\033[22;34;1m"<<"\t[Goalie] diveType: long dive left"<<"\033[0m" << std::endl; \ + else if(x == 3) std::cerr << "\033[22;34;1m"<<"\t[Goalie] diveType: long dive right"<<"\033[0m" << std::endl; \ + else if(x == 4) std::cerr << "\033[22;34;1m"<<"\t[Goalie] diveType: close dive left"<<"\033[0m" << std::endl; \ + else if(x == 5) std::cerr << "\033[22;34;1m"<<"\t[Goalie] diveType: close dive right"<<"\033[0m" << std::endl; \ + else if(x == 6) std::cerr << "\033[22;34;1m"<<"\t[Goalie] diveType: stop ball"<<"\033[0m" << std::endl; \ + option GoalieLearner { private: @@ -152,7 +160,7 @@ option GoalieLearner if ((Timestamp() - lastTimeRoleSent).getMs() > 1000.0) { cerr << "\033[22;34;1m\t[Goalie] DiveHandle::ballProjectionEstimate: " << theDiveHandle.ballProjectionEstimate << " \033[0m" << endl; - cerr << "\033[22;34;1m\t[Goalie] DiveHandle::diveType: " << theDiveHandle.diveType << " \033[0m" << endl; + DIVE_TYPE( (int)theDiveHandle.diveType ); cerr << "\033[0;32;1m\t[Goalie] DiveHandle::diveTimer: " << theDiveHandle.diveTime << " \033[0m" << endl; } #endif From b8f062984bb2595340e517ca96a781de775f4ed5 Mon Sep 17 00:00:00 2001 From: Francesco Riccio Date: Thu, 5 Dec 2013 15:27:32 +0100 Subject: [PATCH 2/6] r2 updated module --- naoqi/modules/R2module/config/dh_head.cfg | 3 +- naoqi/modules/R2module/config/dh_leftArm.cfg | 12 +- naoqi/modules/R2module/config/dh_rightArm.cfg | 15 +- .../modules/R2module/include/libmath/Rutils.h | 2 + naoqi/modules/R2module/include/r2Module.h | 6 +- naoqi/modules/R2module/include/task.h | 27 ++- naoqi/modules/R2module/src/configReader.cpp | 5 +- naoqi/modules/R2module/src/libmath/Rutils.cpp | 39 +++- .../modules/R2module/src/libmath/kinChain.cpp | 9 +- .../R2module/src/libmath/transform.cpp | 2 +- naoqi/modules/R2module/src/r2Module.cpp | 180 +++++++++++++---- naoqi/modules/R2module/src/task.cpp | 191 +++++++++++++++--- 12 files changed, 391 insertions(+), 100 deletions(-) diff --git a/naoqi/modules/R2module/config/dh_head.cfg b/naoqi/modules/R2module/config/dh_head.cfg index f94325d..7c2cc03 100644 --- a/naoqi/modules/R2module/config/dh_head.cfg +++ b/naoqi/modules/R2module/config/dh_head.cfg @@ -1,7 +1,8 @@ [ chain ] = { -T 0 0 126.5 H 0 -M_PI/2 0 q1 H 0 M_PI/2 0 q2 T 53.9 0 67.9 } +# CoM-head shift +T 0 0 126.5 diff --git a/naoqi/modules/R2module/config/dh_leftArm.cfg b/naoqi/modules/R2module/config/dh_leftArm.cfg index 5d94fde..8a514cf 100644 --- a/naoqi/modules/R2module/config/dh_leftArm.cfg +++ b/naoqi/modules/R2module/config/dh_leftArm.cfg @@ -1,18 +1,14 @@ [ chain ] = { -T 0 98 100 -R 1 0 0 -M_PI/2 -H 0 0 0 q1 -R 1 0 0 M_PI/2 +H 0 M_PI/2 0 q1 H 0 0 0 q2 -T 105 15 0 R 0 0 1 -M_PI/2 R 1 0 0 -M_PI/2 -H 0 0 0 q3 +H -15 M_PI/2 105 q3 R 0 0 1 M_PI/2 -R 0 1 0 -M_PI/2 H 0 0 0 q4 T 113.7 0 12.31 -R 0 1 0 M_PI/2 +R 0 0 1 -M_PI/2 +R 1 0 0 -M_PI/2 H 0 0 0 q5 } diff --git a/naoqi/modules/R2module/config/dh_rightArm.cfg b/naoqi/modules/R2module/config/dh_rightArm.cfg index 3abd343..06d2d09 100644 --- a/naoqi/modules/R2module/config/dh_rightArm.cfg +++ b/naoqi/modules/R2module/config/dh_rightArm.cfg @@ -1,11 +1,14 @@ [ chain ] = { -T 0 -98 100 100 -H 0 -M_PI/2 0 q1 -H 105 M_PI/2 -15 q2 +H 0 M_PI/2 0 q1 +H 0 0 0 q2 R 0 0 1 -M_PI/2 -H 0 -M_PI/2 0 q3 -T 0 -12.31 0 +R 1 0 0 -M_PI/2 +H 15 M_PI/2 105 q3 +R 0 0 1 M_PI/2 H 0 0 0 q4 -H 0 0 113.7 q5 +T 113.7 0 12.31 +R 0 0 1 -M_PI/2 +R 1 0 0 -M_PI/2 +H 0 0 0 q5 } diff --git a/naoqi/modules/R2module/include/libmath/Rutils.h b/naoqi/modules/R2module/include/libmath/Rutils.h index 0b0e0c4..e63821b 100644 --- a/naoqi/modules/R2module/include/libmath/Rutils.h +++ b/naoqi/modules/R2module/include/libmath/Rutils.h @@ -117,6 +117,8 @@ Eigen::Matrix4d homY(double a, const Eigen::Vector3d& shift); void Hz(double a, const Eigen::Vector3d& shift, Eigen::Matrix4d* mz); Eigen::Matrix4d homZ(double a, const Eigen::Vector3d& shift); +Eigen::Matrix4d hTranslation(const Eigen::Vector3d& shift); + /* * Compute the Moore-Penrose pseudo-inversion of matrix 'm' using SVD decomposition. * Optional value is the default tolerance for the singular values of 'm'. diff --git a/naoqi/modules/R2module/include/r2Module.h b/naoqi/modules/R2module/include/r2Module.h index 5db8d12..0debd41 100644 --- a/naoqi/modules/R2module/include/r2Module.h +++ b/naoqi/modules/R2module/include/r2Module.h @@ -75,9 +75,11 @@ class R2Module : public AL::ALModule // Common base kinematic chain (left foot to body center) Rmath::KinChain* theKinChainLeftLeg; // All initialized tasks - std::map taskMap; + std::map taskMap; // Stack of tasks currently active - std::set taskSet; + std::set taskSet; + // Tasks respective positions within the whole kinematic chain + std::map taskLoc; public: diff --git a/naoqi/modules/R2module/include/task.h b/naoqi/modules/R2module/include/task.h index 946a415..0f45352 100644 --- a/naoqi/modules/R2module/include/task.h +++ b/naoqi/modules/R2module/include/task.h @@ -108,14 +108,39 @@ class Task : public TaskBase // The task kinematic chain Rmath::KinChain* theKinChain; + // Toggle positioning/velocity task + bool positioningActive; + // Toggle joint space/cartesian space control + bool jointControlActive; + + // Discrete path to desired pose + std::vector path; + // Current progress in the path + int path_currentStep; + public: // Constructor Task(int m, int n, int _priority, ConfigReader theConfigReader, int _base, int _ee); // Destructor ~Task(); + // Retrieve the current end-effector pose + Eigen::VectorXd getCurrentPose(const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity()) const; + // Set a desired pose in the task space + void setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints = 1, + const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity()); + // Set a desired configuration in the joint space + void setDesiredConfiguration(const Eigen::VectorXd& desiredConf, int n_controlPoints); + // Reset the current target pose (either in joint or task space) + inline void resetDesiredPose() + { + positioningActive = false; + path.clear(); + } + // Update function - void update(const Eigen::VectorXd& q, double K, const Eigen::VectorXd& desiredPose, const Eigen::VectorXd& desiredVel); + void update(const Eigen::VectorXd& q, const Eigen::VectorXd& desiredVel, + double K = 1.0, const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity()); }; #endif diff --git a/naoqi/modules/R2module/src/configReader.cpp b/naoqi/modules/R2module/src/configReader.cpp index e3305c3..c0ff5a2 100644 --- a/naoqi/modules/R2module/src/configReader.cpp +++ b/naoqi/modules/R2module/src/configReader.cpp @@ -92,7 +92,7 @@ void ConfigReader::dhReader(std::string dh_file_path) else if (tokens.at(0) == "T") { Rmath::Transform* transl = new Rmath::Translation( to_double(tokens.at(1)), - to_double(tokens.at(2)) , + to_double(tokens.at(2)), to_double(tokens.at(3)) ); transformations.push_back(transl); } @@ -105,11 +105,12 @@ void ConfigReader::dhReader(std::string dh_file_path) valueReader(tokens.at(4)) ); transformations.push_back(rot); } + + if (tokens.at(0) == "}") break; } cfg_dh.close(); } - void ConfigReader::paramsReader(std::string params_file_path) { diff --git a/naoqi/modules/R2module/src/libmath/Rutils.cpp b/naoqi/modules/R2module/src/libmath/Rutils.cpp index 4aa5cd2..033300c 100644 --- a/naoqi/modules/R2module/src/libmath/Rutils.cpp +++ b/naoqi/modules/R2module/src/libmath/Rutils.cpp @@ -21,6 +21,7 @@ #include "libmath/transform.h" #define INFO(x) std::cout << "[Rmath] " << x << std::endl; +//#define JACOBIAN_DEBUG std::cout << "[Rmath::Jacobian] " << x << std::endl; /** -------- Core tools -------- */ @@ -386,6 +387,23 @@ Eigen::Matrix4d Rmath::homZ(double a, const Eigen::Vector3d& shift) return hz; } +Eigen::Matrix4d Rmath::hTranslation(const Eigen::Vector3d& shift) +{ + Eigen::Matrix4d hz; + // Using Eigen comma initializer + hz << 1, 0, 0, shift(0), + 0, 1, 0, shift(1), + 0, 0, 1, shift(2), + 0, 0, 0, 1; + +#ifdef CROP_TO_TOLERANCE + // Trim according to default numerical tolerance + trim(&hz); +#endif + + return hz; +} + /* * Compute the Moore-Penrose pseudo-inversion of matrix 'm' using SVD decomposition. * Optional value is the default tolerance for the singular values of 'm'. @@ -506,7 +524,8 @@ void Rmath::DirectKin(const std::vector& chain, Eigen::Matrix (*H) = Eigen::Matrix4d::Identity(); // Go through the chain, compute i-to-i+1 transformation and post-multiply - for (unsigned int i=0; i < chain.size(); ++i) (*H) *= chain.at(i)->transform(); + for (unsigned int i=0; i < chain.size(); ++i) + (*H) *= chain.at(i)->transform(); #ifdef CROP_TO_TOLERANCE // Trim according to default numerical tolerance @@ -582,6 +601,13 @@ void Rmath::geomJacobian(const std::vector& chain, const std: // Initialization of J as a zero-matrix (*J) = Eigen::MatrixXd::Zero(taskSpaceDim, i_joints.size()); +//#ifdef JACOBIAN_DEBUG +// INFO( "\ttask dimension"<< taskSpaceDim ); +// for(int i=0; i< chain.size(); ++i) chain.at(i)->print(std::cout) ; +// INFO( "\tnumber of joints: "<< i_joints.size() ); +// for(int i=0; i< i_joints.size(); ++i) INFO( "\ti_joints: "<< i_joints.at(i) ); +//#endif + // Default Z-axis in the base frame [ 0 , 0 , 1 ]' Eigen::Vector3d z = Eigen::Vector3d::UnitZ(); @@ -591,22 +617,31 @@ void Rmath::geomJacobian(const std::vector& chain, const std: // Base-EE translation vector Eigen::Vector3d d_0e = H_ee.topRightCorner(3, 1); + // Jacobian leftmost column Eigen::VectorXd J0(6,1); J0 << z.cross(d_0e), z; // Crop to required task space dimension J->col(0) = J0.head(taskSpaceDim); +//#ifdef JACOBIAN_DEBUG +// INFO( "\tendeffector translation d0e: \n"<< H_ee.topRightCorner(3, 1) ); +//#endif + // Go through the chain of transforms and build up J column-wise for (int i=1; i < i_joints.size(); ++i) { // Consider the subchain of transformation leading to joint i-1 Eigen::Matrix4d currentH; std::vector currentSubchain; //(chain.begin(), chain.begin()+i_joints.at(i-1)); - for (int j=0; j <= i_joints.at(i)-1; ++j) + for (int j=0; j < i_joints.at(i); ++j) currentSubchain.push_back( chain.at(j) ); + // Compute forward kinematics up to joint i-1 DirectKin(currentSubchain, ¤tH); +//#ifdef JACOBIAN_DEBUG +// INFO( "\tKinChain joint: "< q_indices; - int i = 0; std::map::iterator selector = joints.begin(); + // Iterate over the joint container while(selector != joints.end()) { // Skip joints out of the desired range - if ( (i >= base) && (i <= ee) ) - q_indices.push_back(selector->first); - ++selector; ++i; + if ( selector->first >= base && selector->first <= ee ) + q_indices.push_back(selector->first -base); + + ++selector; } // Compute the Jacobian matrix diff --git a/naoqi/modules/R2module/src/libmath/transform.cpp b/naoqi/modules/R2module/src/libmath/transform.cpp index c230ee1..af11861 100644 --- a/naoqi/modules/R2module/src/libmath/transform.cpp +++ b/naoqi/modules/R2module/src/libmath/transform.cpp @@ -100,7 +100,7 @@ void Rmath::Translation::parameters(std::map* params) */ Eigen::Matrix4d Rmath::Translation::transform() const { - return Rmath::homZ(0.0, values); + return Rmath::hTranslation(values); } /* diff --git a/naoqi/modules/R2module/src/r2Module.cpp b/naoqi/modules/R2module/src/r2Module.cpp index d7ea68c..e11ac63 100644 --- a/naoqi/modules/R2module/src/r2Module.cpp +++ b/naoqi/modules/R2module/src/r2Module.cpp @@ -11,13 +11,13 @@ /** TODO: - - check transformations + - force soth convergence (stop criteria) - task formalization */ #define INFO(x) std::cerr << "\033[22;34;1m" << "[r2module] " << x << "\033[0m" << std::endl; -//#define DEBUG_MODE -#define TEST_KINCHAIN +#define DEBUG_MODE +//#define TEST_KINCHAIN // Nao joints number #define JOINTS_NUM 24 @@ -37,7 +37,7 @@ namespace AL { // Config (.cfg) files directory and paths -static const std::string CONFIG_PATH = "/../config/"; +static const std::string CONFIG_PATH = "../config/"; static const std::string JOINT_BOUNDS_CFG = "joints_params.cfg"; static const std::string LEFT_LEG_CFG = "dh_leftLeg.cfg"; static const std::string LEFT_ARM_CFG = "dh_leftArm.cfg"; @@ -198,27 +198,39 @@ void R2Module::init() INFO("Initializing: retrieving joint limits... "); #endif - /* ------------------------------ Other tasks: head (priority = 1), left arm (priority = 2) ------------------------------ */ + /* ------------------------------ Other tasks: head (priority = 2), left arm (priority = 1) ------------------------------ */ // Extract joint bounds from the configuration file ConfigReader headConfig( CONFIG_PATH + HEAD_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); // Task initialization - Task* head = new Task(3, HEAD, 1, headConfig, 0, HEAD); + Task* head = new Task(3, HEAD, 2, headConfig, 0, HEAD); // Extract joint bounds from the configuration file - ConfigReader pointingConfig( CONFIG_PATH + LEFT_ARM_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); + ConfigReader LpointingConfig( CONFIG_PATH + LEFT_ARM_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); // Task initialization - Task* pointing = new Task(3, L_ARM, 2, pointingConfig, HEAD + R_ARM, HEAD + R_ARM + L_ARM); + Task* Lpointing = new Task(3, L_ARM, 1, LpointingConfig, HEAD + R_ARM, HEAD + R_ARM + L_ARM); + + // Extract joint bounds from the configuration file + ConfigReader RpointingConfig( CONFIG_PATH + RIGHT_ARM_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); + // Task initialization + Task* Rpointing = new Task(3, R_ARM, 3, RpointingConfig, HEAD, HEAD + R_ARM); // Push every task into the task map taskMap.insert( std::pair ("Head task", head) ); - taskMap.insert( std::pair ("Left arm task", pointing) ); + taskMap.insert( std::pair ("Left arm task", Lpointing) ); + taskMap.insert( std::pair ("Right arm task", Rpointing) ); + + // Storing tasks location with respect to the whole Nao kinematic chain + taskLoc.insert( std::pair (head, 0) ); + taskLoc.insert( std::pair (Lpointing, HEAD + R_ARM) ); + taskLoc.insert( std::pair (Rpointing, HEAD) ); #ifdef DEBUG_MODE INFO("Initializing tasks: "); INFO("- Joint limits, with priority " << jointLimits->getPriority()); INFO("- Head task, with priority " << head->getPriority()); - INFO("- Left arm task, with priority " << pointing->getPriority()); + INFO("- Left arm task, with priority " << Lpointing->getPriority()); + INFO("- Right arm task, with priority " << Rpointing->getPriority()); #endif } @@ -338,10 +350,62 @@ void R2Module::motion() INFO("Executing main loop..."); #endif + // Shift CoM-left arm + Eigen::Vector3d CoM_LA; + CoM_LA << 0.0, 98.0, 100.0; + // Shift CoM-right arm + Eigen::Vector3d CoM_RA; + CoM_RA << 0.0, -98.0, 100.0; + + // Defining the desired pose vectors in the task space + Eigen::VectorXd desiredHeadPose(3), desiredLHandPose(3), desiredRHandPose(3); + Eigen::VectorXd desiredLHandConf(L_ARM), desiredRHandConf(R_ARM); + + //desiredHeadPose << 53.9, 0.0, 194.4; // zero-position HEAD + desiredHeadPose << 0.0, 100.0, 0.0; + +// desiredLHandPose << 218.7, 133, 112.31; // zero-position L ARM + desiredLHandPose << -15.0, 316.0, 112.31; // point-left L ARM +// desiredLHandPose << 0, -100, -100; // point L ARM +// desiredLHandPose << -15.0, 98.0, 329.01; + desiredLHandConf = Eigen::VectorXd::Zero(desiredLHandConf.size()); + desiredLHandConf(1) = M_PI/2; +// desiredLHandConf(3) = -M_PI/2; + +// desiredLHandPose << 250, -250, 250; // +X +// desiredLHandPose << 0.0, 250.0, 0.0; // -Y +// desiredLHandPose << 0.0, 0.0, 250; // +Z + + +// desiredRHandPose << 218.7, -133, 112.31; //zero-position R ARM + desiredRHandPose << 15.0, -316.0, 112.31; //point-right R ARM +// desiredRHandPose << 0, -100, -100; // point R ARM +// desiredRHandPose << -15.0, -98.0, 329.01; + desiredRHandConf = Eigen::VectorXd::Zero(desiredRHandConf.size()); + desiredRHandConf(1) = -M_PI/2; +// desiredRHandConf(3) = M_PI/2; + +// desiredRHandPose << 250, -250, -250; // +X +// desiredRHandPose << 0.0, -250.0, 0.0; // -Y +// desiredRHandPose << 0.0, 0.0, 250; // +Z + + taskMap["Head task"]->setDesiredPose(desiredHeadPose, 1, Rmath::hTranslation(Eigen::Vector3d::Zero())); +// taskMap["Left arm task"]->setDesiredPose(desiredLHandPose, 15, Rmath::homX(-M_PI/2, CoM_LA) ); + taskMap["Left arm task"]->setDesiredConfiguration(desiredLHandConf, 3); +// taskMap["Right arm task"]->setDesiredPose(desiredRHandPose, 15, Rmath::homX(-M_PI/2, CoM_RA) ); + taskMap["Right arm task"]->setDesiredConfiguration(desiredRHandConf, 3); + +#ifdef DEBUG_MODE + INFO("Desired head position in space: [\n" << desiredHeadPose << "]"); + INFO("Desired left hand position in space: [\n" << desiredLHandPose << "]"); + INFO("Desired right hand position in space: [\n" << desiredRHandPose << "]"); +#endif + // Turn tasks to active jointLimits->activate(); taskMap["Head task"]->activate(); taskMap["Left arm task"]->activate(); + taskMap["Right arm task"]->activate(); // Main loop while(true) @@ -374,6 +438,8 @@ void R2Module::motion() INFO("Initializing HQP solver..."); INFO( "Active tasks: " << (taskSet.size()+1) << " out of " << (taskMap.size()+1) ); INFO("Generating stack..."); + + int task_i = 2; #endif // Set up the hierarchy std::vector A; @@ -382,12 +448,25 @@ void R2Module::motion() A.push_back(jointLimits->constraintMatrix()); b.push_back(jointLimits->vectorBounds()); // All the remaining tasks (in descending order of priority) - std::set::iterator updater = taskSet.begin(); + std::set::iterator updater = taskSet.begin(); while( updater != taskSet.end() ) { - A.push_back((*updater)->constraintMatrix()); + // Current task constraint matrix (restricted to the joint involved) + Eigen::MatrixXd currentA_task = (*updater)->constraintMatrix(); + + // Rewrite the task constraint matrix with the complete joint configuration + Eigen::MatrixXd currentA = Eigen::MatrixXd::Zero( currentA_task.rows(), JOINTS_NUM ); + // The task constraint matrix is the only non-zero block in A + currentA.block( 0, taskLoc[*updater], currentA_task.rows(), currentA_task.cols() ) = currentA_task; + + A.push_back(currentA); b.push_back((*updater)->vectorBounds()); ++updater; + +#ifdef DEBUG_MODE + INFO("Task n. " << task_i << ", constraint matrix (enlarged):\n" << currentA); + ++task_i; +#endif } hsolver.pushBackStages(A, b); @@ -399,23 +478,24 @@ void R2Module::motion() INFO("Configuring HQP solver..."); INFO("Start active search... "); #endif -// // Run the solver -// hsolver.activeSearch(qdot); -// // Trim ssolution -// Rmath::trim(&qdot); - -//#ifdef DEBUG_MODE -// INFO("Done."); -// INFO("Active set: "); -// hsolver.showActiveSet(std::cerr); -// INFO("Solution: [\n" << qdot << "]"); -//#endif + // Run the solver + hsolver.activeSearch(qdot); + // Trim ssolution + Rmath::trim(&qdot); + +#ifdef DEBUG_MODE + INFO("Done."); + INFO("Active set: "); + hsolver.showActiveSet(std::cerr); + INFO("Solution: [\n" << qdot << "]"); +#endif #ifdef TEST_KINCHAIN qdot = Eigen::VectorXd::Zero(q.size()); Eigen::MatrixXd J_TMP_pinv; Rmath::pseudoInverse(J_TMP, &J_TMP_pinv); - qdot.segment(HEAD + R_ARM) = J_TMP_pinv * r; + + qdot.segment(HEAD + R_ARM /*+ 1*/) = J_TMP_pinv * r; INFO("Solution: [\n" << qdot << "]"); #endif @@ -433,9 +513,16 @@ Eigen::VectorXd R2Module::getConfiguration() // Store the resulting values in a Eigen::VectorXd Eigen::VectorXd currentConfiguration(jointID.size()); + AL::ALValue jointId, angles; + // Use Naoqi ID for each joint to request the joint current measured value for(int i=0; i getAngles(jointID.at(i), true) ).at(0); + jointId.arrayPush( jointID.at(i) ); + + angles = fMotionProxy->getAngles( jointId, true ); + + for(int i=0; i update(q_TMP); theKinChain_TMP->forward(&H_TMP); theKinChain_TMP->differential(&J_TMP, 3); + INFO("Direct kinematics transform: "); INFO(std::endl << H_TMP); INFO("Jacobian (linear velocities only): "); INFO(std::endl << J_TMP); #endif - /* -------------------- Head and left arm task update --------------------*/ + /* -------------------- Head and arms tasks update --------------------*/ - // Defining the desired pose vectors in the task space - Eigen::VectorXd desiredHeadPose(3), desiredLHandPose(3); + // Shift CoM-left arm + Eigen::Vector3d CoM_LA; + CoM_LA << 0.0, 98.0, 100.0; + // Shift CoM-right arm + Eigen::Vector3d CoM_RA; + CoM_RA << 0.0, -98.0, 100.0; - //desiredHeadPose << 53.9, 0.0, 194.4; // zero-position HEAD - desiredHeadPose << -20.0, 0.0, 194.4; - - //desiredLHandPose << 218.7, 133, 112.31; // zero-position L ARM -// desiredLHandPose << 0.0, 329.01, 112.31; // point-left L ARM - desiredLHandPose << 15.1, 112.31, 316.0; // point-up L ARM - - //desiredRHandPose << 218.7, -133, 112.31, 0.0, 0.0, 0.0; //zero-position R ARM - //desiredRHandPose <<-15, -329.01, 112.31, 0.0, 0.0, 0.0; //point-right R ARM // Head task update - ( taskMap["Head task"] )->update( q.head(HEAD), K_HEAD, desiredHeadPose, Eigen::VectorXd::Zero(3) ); + ( taskMap["Head task"] )->update( q.head(HEAD), Eigen::VectorXd::Zero(3), K_HEAD, Rmath::hTranslation(Eigen::Vector3d::Zero())); #ifdef DEBUG_MODE - INFO("Desired head position in space: [\n" << desiredHeadPose << "]"); INFO("Head task constraint equation: "); INFO(std::endl << *( taskMap["Head task"] ) ); #endif // Left arm task update - ( taskMap["Left arm task"] )->update( q.segment(HEAD+R_ARM), K_HEAD, desiredLHandPose, Eigen::VectorXd::Zero(3) ); +// ( taskMap["Left arm task"] )->update( q.segment(HEAD+R_ARM), Eigen::VectorXd::Zero(3), K_HEAD, Rmath::homX(-M_PI/2, CoM_LA) ); + ( taskMap["Left arm task"] )->update( q.segment(HEAD+R_ARM), Eigen::VectorXd::Zero(L_ARM), K_HEAD); #ifdef DEBUG_MODE - INFO("Desired left hand position in space: [\n" << desiredLHandPose << "]"); INFO("Left arm task constraint equation: "); INFO(std::endl << *( taskMap["Left arm task"] ) ); #endif + // Right arm task update +// ( taskMap["Right arm task"] )->update( q.segment(HEAD), Eigen::VectorXd::Zero(3), K_HEAD, Rmath::homX(-M_PI/2, CoM_RA) ); + ( taskMap["Right arm task"] )->update( q.segment(HEAD), Eigen::VectorXd::Zero(R_ARM), K_HEAD); + +#ifdef DEBUG_MODE + INFO("Right arm task constraint equation: "); + INFO(std::endl << *( taskMap["Right arm task"] ) ); +#endif + #ifdef TEST_KINCHAIN INFO("Desired left hand position in space: [\n" << desiredLHandPose << "]"); - r = K_HEAD * ( desiredLHandPose - H_TMP.topRightCorner(3,1) ); +// r = K_HEAD * ( desiredLHandPose - H_TMP.topRightCorner(3,1) ) + Eigen::Vector3d::UnitZ()*100; + r = ( Eigen::Vector3d::UnitX()*100 + + Eigen::Vector3d::UnitY()*100 + + Eigen::Vector3d::UnitZ()*100 ); INFO("New target velocity:\n[" << r << "]"); #endif @@ -526,6 +621,7 @@ void R2Module::updateConstraints(const Eigen::VectorXd& q) */ bool R2Module::updateConfiguration(const Eigen::VectorXd& delta_q) { + #ifdef TEST_KINCHAIN Eigen::VectorXd delta_q_ = delta_q; #endif diff --git a/naoqi/modules/R2module/src/task.cpp b/naoqi/modules/R2module/src/task.cpp index a647070..8983f9f 100644 --- a/naoqi/modules/R2module/src/task.cpp +++ b/naoqi/modules/R2module/src/task.cpp @@ -13,6 +13,8 @@ #define POSITION_TASK_DIM 3 +#define POSE_ERROR_TOLERANCE 50 /* [mm] */ + /** ---------- TaskBase ---------- */ @@ -236,7 +238,8 @@ Task::Task(int m, int n, int _priority, ConfigReader theConfigReader, int _base, // Base class initalization TaskBase(_priority, Eigen::MatrixXd::Identity(m, n), Eigen::VectorXd::Zero(m), soth::Bound::BOUND_TWIN ), // Kinematic chain initialization - theKinChain( new Rmath::KinChain( theConfigReader, _base, _ee) ) + theKinChain( new Rmath::KinChain( theConfigReader, _base, _ee) ), + positioningActive(false), jointControlActive(false) { #ifdef DEBUG_MODE INFO("Task kinematic chain:"); @@ -252,54 +255,180 @@ Task::~Task() if(theKinChain) delete theKinChain; } + +Eigen::VectorXd Task::getCurrentPose(const Eigen::Matrix4d& baseTransform) const +{ + Eigen::Matrix4d H_chain; + theKinChain->forward((&H_chain)); + H_chain = baseTransform * H_chain; + Eigen::VectorXd currentPose (constraint_matrix.rows()); + if(constraint_matrix.rows() > POSITION_TASK_DIM) + // Retrieving the position from the translation vector of the forward kinematics + // Retrieving the orientation from the Euler fixed frame x-y-z angles + currentPose << H_chain.topRightCorner(POSITION_TASK_DIM,1), + Rmath::xyzEulerAngles( H_chain.topLeftCorner(3,3) ).head(constraint_matrix.rows()-POSITION_TASK_DIM); + // If the task target is a position vector (A row size <= 3) just the translation vector of the forward kinematics is needed + else + currentPose << H_chain.col(POSITION_TASK_DIM).head(constraint_matrix.rows()); + + return currentPose; +} + + +void Task::setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints, const Eigen::Matrix4d& baseTransform) +{ + // Re-computing direct kinematics + Eigen::Matrix4d H_chain; + theKinChain->forward((&H_chain)); + H_chain = baseTransform * H_chain; + + Eigen::VectorXd initialPose (constraint_matrix.rows()); + if(constraint_matrix.rows() > POSITION_TASK_DIM) + // Retrieving the position from the translation vector of the forward kinematics + // Retrieving the orientation from the Euler fixed frame x-y-z angles + initialPose << H_chain.topRightCorner(POSITION_TASK_DIM,1), + Rmath::xyzEulerAngles( H_chain.topLeftCorner(3,3) ).head(constraint_matrix.rows()-POSITION_TASK_DIM); + // If the task target is a position vector (A row size <= 3) just the translation vector of the forward kinematics is needed + else + initialPose << H_chain.col(POSITION_TASK_DIM).head(constraint_matrix.rows()); + + assert(dp.size() == initialPose.size()); + + for (float i = 1.0; i <= n_controlPoints; ++i) + path.push_back(initialPose + (i/static_cast(n_controlPoints)) * (dp - initialPose)); + + path_currentStep = 0; + positioningActive = true; + if (jointControlActive) jointControlActive = false; +} + +void Task::setDesiredConfiguration(const Eigen::VectorXd& desiredConf, int n_controlPoints) +{ + Eigen::VectorXd initialConf = theKinChain->jointConfiguration(); + assert(initialConf.size() == desiredConf.size()); + + constraint_matrix = Eigen::MatrixXd::Identity(initialConf.size(), initialConf.size()); + bounds.setZero(initialConf.size(),1); + + for (float i = 1.0; i <= n_controlPoints; ++i) + path.push_back(initialConf + (i/static_cast(n_controlPoints)) * (desiredConf - initialConf)); + + path_currentStep = 0; + positioningActive = true; + if (!jointControlActive) jointControlActive = true; +} + /* * Task update function: update the constraint matrix A=A(q) with a new value of q and replace the target vector * with a proportional/derivative control law of the type K*POSE_ERROR + DESIRED_VELOCITY. */ -void Task::update(const Eigen::VectorXd& q, double K, const Eigen::VectorXd& desiredPose, const Eigen::VectorXd& desiredVel) + +void Task::update( const Eigen::VectorXd& q, const Eigen::VectorXd& desiredVel, double K, const Eigen::Matrix4d& baseTransform ) { // Dimensions must be consistent - assert(constraint_matrix.rows() == desiredPose.size()); assert(constraint_matrix.rows() == desiredVel.size()); assert(constraint_matrix.cols() == q.size()); + // Cartesian space task + if (!jointControlActive) + { #ifdef DEBUG_MODE - INFO("Updating task..."); + INFO("Updating task..."); #endif - // Updating the task kinematic chain with the new joint values - theKinChain->update(q); - - // Re-computing direct kinematics - Eigen::Matrix4d H_chain; - theKinChain->forward((&H_chain)); - // Re-computing differential kinematics - Eigen::MatrixXd J_chain(constraint_matrix.rows(), constraint_matrix.cols()); - theKinChain->differential(&J_chain,constraint_matrix.rows()); - - // Replacing the task constraint matrix with the updated version - constraint_matrix << J_chain; + // Updating the task kinematic chain with the new joint values + theKinChain->update(q); + + // Re-computing differential kinematics + Eigen::MatrixXd J_chain(constraint_matrix.rows(), constraint_matrix.cols()); + theKinChain->differential(&J_chain,constraint_matrix.rows()); + + // Replacing the task constraint matrix with the updated version + if (constraint_matrix.rows() > POSITION_TASK_DIM) + { + // Computing base transform + Eigen::MatrixXd baseT = Eigen::MatrixXd::Zero(6,6); + baseT.topLeftCorner(3,3) = baseTransform.topLeftCorner(3,3); + baseT.bottomRightCorner(3,3) = baseTransform.topLeftCorner(3,3); + // Pre-multipling base transform + constraint_matrix << (baseT * J_chain).topRows(constraint_matrix.rows()); + } + else + { + // Pre-multipling base transform + constraint_matrix << (baseTransform.topLeftCorner(3,3) * J_chain).topRows(constraint_matrix.rows()); + } #ifdef DEBUG_MODE - INFO("New constraint matrix:"); - INFO(constraint_matrix); + INFO("New constraint matrix:"); + INFO(constraint_matrix); #endif - // Computing the current pose in the task space - Eigen::VectorXd currentPose(constraint_matrix.rows()); - // If the task target is a pose (position+orientation) vector, a minimal description of the orientation has to be computed - if(constraint_matrix.rows() > POSITION_TASK_DIM) - // Retrieving the position from the translation vector of the forward kinematics - // Retrieving the orientation from the Euler fixed frame x-y-z angles - currentPose << H_chain.topRightCorner(POSITION_TASK_DIM,1), - Rmath::xyzEulerAngles( H_chain.topLeftCorner(3,3) ).head(constraint_matrix.rows()-POSITION_TASK_DIM); - // If the task target is a position vector (A row size <= 3) just the translation vector of the forward kinematics is needed + if (positioningActive) + { + // Re-computing direct kinematics + Eigen::Matrix4d H_chain; + theKinChain->forward((&H_chain)); + H_chain = baseTransform * H_chain; + + // Computing the current pose in the task space + Eigen::VectorXd currentPose(constraint_matrix.rows()); + // If the task target is a pose (position+orientation) vector, a minimal description of the orientation has to be computed + if(constraint_matrix.rows() > POSITION_TASK_DIM) + // Retrieving the position from the translation vector of the forward kinematics + // Retrieving the orientation from the Euler fixed frame x-y-z angles + currentPose << H_chain.topRightCorner(POSITION_TASK_DIM,1), + Rmath::xyzEulerAngles( H_chain.topLeftCorner(3,3) ).head(constraint_matrix.rows()-POSITION_TASK_DIM); + // If the task target is a position vector (A row size <= 3) just the translation vector of the forward kinematics is needed + else + currentPose << H_chain.col(POSITION_TASK_DIM).head(constraint_matrix.rows()); + + Eigen::VectorXd pose_error = path.at(path_currentStep) - currentPose; + + // Updating the bound vector with the task error + a feedforward term + for(int i=0; iupdate(q); - // Updating the bound vector with the task error + a feedforward term - for(int i=0; ijointConfiguration(); + + // Updating the bound vector with the task error + a feedforward term + for(int i=0; i Date: Fri, 6 Dec 2013 21:55:34 +0100 Subject: [PATCH 3/6] R2module:: general updates && added configParams.h --- naoqi/modules/R2module/include/configParams.h | 128 ++++++++++ .../R2module/include/libmath/kinChain.h | 1 + naoqi/modules/R2module/include/r2Module.h | 7 + naoqi/modules/R2module/include/task.h | 4 + naoqi/modules/R2module/src/r2Module.cpp | 224 ++++++++++-------- naoqi/modules/R2module/src/task.cpp | 50 +++- 6 files changed, 315 insertions(+), 99 deletions(-) create mode 100644 naoqi/modules/R2module/include/configParams.h diff --git a/naoqi/modules/R2module/include/configParams.h b/naoqi/modules/R2module/include/configParams.h new file mode 100644 index 0000000..ee11c96 --- /dev/null +++ b/naoqi/modules/R2module/include/configParams.h @@ -0,0 +1,128 @@ +#ifndef CONFIG_PARAMS +#define CONFIG_PARAMS + +#include + +// CIRCLE:: n°circles, z_shift, radius + +// Nao joints number +static const int JOINTS_NUM = 24; + +// Discrete integration time step +static const double TIME_STEP = 1.0; + +// Config (.cfg) files directory and paths +//static const std::string CONFIG_PATH = "/home/claudio/Naoqi/NaoModules/r2module/config/"; +static const std::string CONFIG_PATH = "/home/francesco/naoqi/modules/R2module/config/"; +static const std::string JOINT_BOUNDS_CFG = "joints_params.cfg"; +static const std::string LEFT_LEG_CFG = "dh_leftLeg.cfg"; +static const std::string LEFT_ARM_CFG = "dh_leftArm.cfg"; +static const std::string RIGHT_LEG_CFG = "dh_rightLeg.cfg"; +static const std::string RIGHT_ARM_CFG = "dh_rightArm.cfg"; +static const std::string HEAD_CFG = "dh_head.cfg"; + + +/** ---------------------- Head task parameters ---------------------- */ + + +static const int HEAD_CHAIN_BEGIN = 0; +static const int HEAD_CHAIN_SIZE = 2; +static const int HEAD_CHAIN_END = HEAD_CHAIN_BEGIN + HEAD_CHAIN_SIZE; + +static const int HEAD_TASK_DIM = 3; +static const int HEAD_TASK_PRIORITY = 3; +static const int K_HEAD = 1; +static const int HEAD_TASK_NSTEPS = 1; + +static const double HEAD_DESIRED_X = 53.9; +static const double HEAD_DESIRED_Y = 0.0; +static const double HEAD_DESIRED_Z = 194.4; +static const double HEAD_DESIRED_ROLL = 0.0; +static const double HEAD_DESIRED_PITCH = 0.0; +static const double HEAD_DESIRED_YAW = 0.0; + + +/** -------------------- Right arm task parameters -------------------- */ + + +static const int RARM_CHAIN_BEGIN = HEAD_CHAIN_END; +static const int RARM_CHAIN_SIZE = 5; +static const int RARM_CHAIN_END = RARM_CHAIN_BEGIN + RARM_CHAIN_SIZE; + +static const double COM_RARM_SHIFT_X = 0.0; +static const double COM_RARM_SHIFT_Y = -98.0; +static const double COM_RARM_SHIFT_Z = 100.0; + +static const int RARM_TASK_DIM = 3; +static const int RARM_TASK_PRIORITY = 1; +static const int K_RARM = 1; +static const int RARM_TASK_NSTEPS = 30; + +static const double RARM_DESIRED_X = 250.0; +static const double RARM_DESIRED_Y = -113.0; +static const double RARM_DESIRED_Z = 120.0; +static const double RARM_DESIRED_ROLL = 0.0; +static const double RARM_DESIRED_PITCH = 0.0; +static const double RARM_DESIRED_YAW = 0.0; + + +#define RARM_LARM_JOINT_TASK +enum TaskType { + MIRROR_TASK, + MIMIC_TASK +}; +static const TaskType ARMS_TASK = MIMIC_TASK; +static const double MINIMUM_HANDS_DISTANCE = 50.0; + +/** -------------------- Left arm task parameters -------------------- */ + + +static const int LARM_CHAIN_BEGIN = RARM_CHAIN_END; +static const int LARM_CHAIN_SIZE = 5; +static const int LARM_CHAIN_END = LARM_CHAIN_BEGIN + LARM_CHAIN_SIZE; + +static const double COM_LARM_SHIFT_X = 0.0; +static const double COM_LARM_SHIFT_Y = 98.0; +static const double COM_LARM_SHIFT_Z = 100.0; + +static const int LARM_TASK_DIM = 3; +static const int LARM_TASK_PRIORITY = 1; +static const int K_LARM = 1; +static const int LARM_TASK_NSTEPS = 30; + +static const double LARM_DESIRED_X = 0.0; +static const double LARM_DESIRED_Y = 250 + 113.0; +static const double LARM_DESIRED_Z = 120.0; +static const double LARM_DESIRED_ROLL = 0.0; +static const double LARM_DESIRED_PITCH = 0.0; +static const double LARM_DESIRED_YAW = M_PI/2; + + +/** -------------------- Right leg task parameters -------------------- */ + + +static const int RLEG_CHAIN_BEGIN = LARM_CHAIN_END; +static const int RLEG_CHAIN_SIZE = 6; +static const int RLEG_CHAIN_END = RLEG_CHAIN_BEGIN + RLEG_CHAIN_SIZE; + + +/** -------------------- Left leg task parameters -------------------- */ + + +static const int LLEG_CHAIN_BEGIN = RLEG_CHAIN_END; +static const int LLEG_CHAIN_SIZE = 6; +static const int LLEG_CHAIN_END = LLEG_CHAIN_BEGIN + LLEG_CHAIN_SIZE; + +static const int LLEG_TASK_DIM = 3; + + +#ifdef TEST_KINCHAIN +static Rmath::KinChain* theKinChain_TMP; +static Eigen::Matrix4d H_TMP; +static Eigen::MatrixXd J_TMP; +static Eigen::VectorXd q_TMP(LARM_CHAIN_SIZE); +static Eigen::Vector3d r; +#endif + + +#endif diff --git a/naoqi/modules/R2module/include/libmath/kinChain.h b/naoqi/modules/R2module/include/libmath/kinChain.h index 684f458..209d635 100644 --- a/naoqi/modules/R2module/include/libmath/kinChain.h +++ b/naoqi/modules/R2module/include/libmath/kinChain.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include "configReader.h" diff --git a/naoqi/modules/R2module/include/r2Module.h b/naoqi/modules/R2module/include/r2Module.h index 0debd41..eab505c 100644 --- a/naoqi/modules/R2module/include/r2Module.h +++ b/naoqi/modules/R2module/include/r2Module.h @@ -22,6 +22,7 @@ #include #include #include +#include // Soth includes #include @@ -53,6 +54,7 @@ class R2Module : public AL::ALModule boost::shared_ptr fCamProxy; boost::shared_ptr fMotionProxy; boost::shared_ptr fPostureProxy; + boost::shared_ptr fBallTrackerProxy; // Client name for the Video Device. std::string fVideoClientName; @@ -104,6 +106,11 @@ class R2Module : public AL::ALModule void motion(); void vision(); + void closeHand(const std::string& handID); + void openHand(const std::string& handID); + + Eigen::Vector3d getRedBallPosition(); + // Retrieve the current joint configuration Eigen::VectorXd getConfiguration(); // Update every active task with the current joint configuration diff --git a/naoqi/modules/R2module/include/task.h b/naoqi/modules/R2module/include/task.h index 0f45352..79be1e3 100644 --- a/naoqi/modules/R2module/include/task.h +++ b/naoqi/modules/R2module/include/task.h @@ -138,6 +138,10 @@ class Task : public TaskBase path.clear(); } + // design a circular path + void circularPathGenerator( const Eigen::VectorXd& dp, float z_shift = 0.0, int n_controlPoints = 1, float radius = 0.0, int n = 1, + const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity() ); + // Update function void update(const Eigen::VectorXd& q, const Eigen::VectorXd& desiredVel, double K = 1.0, const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity()); diff --git a/naoqi/modules/R2module/src/r2Module.cpp b/naoqi/modules/R2module/src/r2Module.cpp index e11ac63..bf24d7c 100644 --- a/naoqi/modules/R2module/src/r2Module.cpp +++ b/naoqi/modules/R2module/src/r2Module.cpp @@ -8,6 +8,7 @@ #include "r2Module.h" #include "configReader.h" +#include "configParams.h" /** TODO: @@ -16,43 +17,13 @@ */ #define INFO(x) std::cerr << "\033[22;34;1m" << "[r2module] " << x << "\033[0m" << std::endl; -#define DEBUG_MODE +//#define DEBUG_MODE //#define TEST_KINCHAIN -// Nao joints number -#define JOINTS_NUM 24 -// Nao kinematic chain sizes -#define HEAD 2 -#define R_ARM 5 -#define L_ARM 5 -#define R_LEG 6 -#define L_LEG 6 - -// Position task control gain -#define K_HEAD 1 -// Discrete integration time step -#define TIME_STEP 1 namespace AL { -// Config (.cfg) files directory and paths -static const std::string CONFIG_PATH = "../config/"; -static const std::string JOINT_BOUNDS_CFG = "joints_params.cfg"; -static const std::string LEFT_LEG_CFG = "dh_leftLeg.cfg"; -static const std::string LEFT_ARM_CFG = "dh_leftArm.cfg"; -static const std::string RIGHT_LEG_CFG = "dh_rightLeg.cfg"; -static const std::string RIGHT_ARM_CFG = "dh_rightArm.cfg"; -static const std::string HEAD_CFG = "dh_head.cfg"; - -#ifdef TEST_KINCHAIN -static Rmath::KinChain* theKinChain_TMP; -static Eigen::Matrix4d H_TMP; -static Eigen::MatrixXd J_TMP; -static Eigen::VectorXd q_TMP(L_ARM); -static Eigen::Vector3d r; -#endif - /* * Module constructor using Naoqi API methods. */ @@ -122,13 +93,12 @@ void R2Module::init() // Initialize the base kinematic chain (from Nao's left foot to body center) ConfigReader theConfigReader( CONFIG_PATH + LEFT_LEG_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); theConfigReader.storeJointsID(&jointID); - theKinChainLeftLeg = new Rmath::KinChain( theConfigReader, HEAD + R_ARM + L_ARM + R_LEG, - HEAD + R_ARM + L_ARM + R_LEG + L_LEG); + theKinChainLeftLeg = new Rmath::KinChain( theConfigReader, LLEG_CHAIN_BEGIN, LLEG_CHAIN_END ); #ifdef TEST_KINCHAIN // Initialize the test kinematic chain (left arm) ConfigReader theConfigReader_TMP( CONFIG_PATH + LEFT_ARM_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); - theKinChain_TMP = new Rmath::KinChain( theConfigReader_TMP, HEAD + R_ARM, HEAD + R_ARM + L_ARM); + theKinChain_TMP = new Rmath::KinChain( theConfigReader_TMP, LARM_CHAIN_BEGIN, LARM_CHAIN_SIZE); #endif #ifdef DEBUG_MODE @@ -143,6 +113,7 @@ void R2Module::init() fCamProxy = boost::shared_ptr(new ALVideoDeviceProxy(getParentBroker())); fMotionProxy = boost::shared_ptr(new ALMotionProxy(getParentBroker())); fPostureProxy = boost::shared_ptr(new ALRobotPostureProxy(getParentBroker())); + fBallTrackerProxy = boost::shared_ptr(new ALRedBallTrackerProxy(getParentBroker())); } catch (const AL::ALError& e) { @@ -188,7 +159,7 @@ void R2Module::init() theConfigReader.extractJointBounds(&jointBounds); // Convert joint ranges into velocity bound estimates - Eigen::MatrixXd velBounds(JOINTS_NUM,2); + Eigen::MatrixXd velBounds(JOINTS_NUM, 2); posBound2velBound(jointBounds, getConfiguration(), &velBounds); // Actual task initialization @@ -203,17 +174,20 @@ void R2Module::init() // Extract joint bounds from the configuration file ConfigReader headConfig( CONFIG_PATH + HEAD_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); // Task initialization - Task* head = new Task(3, HEAD, 2, headConfig, 0, HEAD); + Task* head = new Task(HEAD_TASK_DIM, HEAD_CHAIN_SIZE, HEAD_TASK_PRIORITY, headConfig, + HEAD_CHAIN_BEGIN, HEAD_CHAIN_END); // Extract joint bounds from the configuration file ConfigReader LpointingConfig( CONFIG_PATH + LEFT_ARM_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); // Task initialization - Task* Lpointing = new Task(3, L_ARM, 1, LpointingConfig, HEAD + R_ARM, HEAD + R_ARM + L_ARM); + Task* Lpointing = new Task(LARM_TASK_DIM, LARM_CHAIN_SIZE, LARM_TASK_PRIORITY, LpointingConfig, + LARM_CHAIN_BEGIN, LARM_CHAIN_END); // Extract joint bounds from the configuration file ConfigReader RpointingConfig( CONFIG_PATH + RIGHT_ARM_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); // Task initialization - Task* Rpointing = new Task(3, R_ARM, 3, RpointingConfig, HEAD, HEAD + R_ARM); + Task* Rpointing = new Task(RARM_TASK_DIM, RARM_CHAIN_SIZE, RARM_TASK_PRIORITY, RpointingConfig, + RARM_CHAIN_BEGIN, RARM_CHAIN_END); // Push every task into the task map taskMap.insert( std::pair ("Head task", head) ); @@ -221,9 +195,9 @@ void R2Module::init() taskMap.insert( std::pair ("Right arm task", Rpointing) ); // Storing tasks location with respect to the whole Nao kinematic chain - taskLoc.insert( std::pair (head, 0) ); - taskLoc.insert( std::pair (Lpointing, HEAD + R_ARM) ); - taskLoc.insert( std::pair (Rpointing, HEAD) ); + taskLoc.insert( std::pair (head, HEAD_CHAIN_BEGIN) ); + taskLoc.insert( std::pair (Lpointing, LARM_CHAIN_BEGIN) ); + taskLoc.insert( std::pair (Rpointing, RARM_CHAIN_BEGIN) ); #ifdef DEBUG_MODE INFO("Initializing tasks: "); @@ -322,6 +296,7 @@ void R2Module::getCurrentFrame() */ void R2Module::vision() { +// fBallTrackerProxy->startTracker(); // Wait a while before starting acquisition qi::os::sleep(1.5); // Main loop @@ -329,7 +304,9 @@ void R2Module::vision() { qi::os::msleep(100); /* WORK IN PROGRESS */ +// INFO("ball position w.r.t. CoM: \n"<stopTracker(); } /* @@ -350,50 +327,59 @@ void R2Module::motion() INFO("Executing main loop..."); #endif - // Shift CoM-left arm - Eigen::Vector3d CoM_LA; - CoM_LA << 0.0, 98.0, 100.0; // Shift CoM-right arm Eigen::Vector3d CoM_RA; - CoM_RA << 0.0, -98.0, 100.0; + CoM_RA << COM_RARM_SHIFT_X, COM_RARM_SHIFT_Y, COM_RARM_SHIFT_Z; + // Shift CoM-left arm + Eigen::Vector3d CoM_LA; + CoM_LA << COM_LARM_SHIFT_X, COM_LARM_SHIFT_Y, COM_LARM_SHIFT_Z; // Defining the desired pose vectors in the task space - Eigen::VectorXd desiredHeadPose(3), desiredLHandPose(3), desiredRHandPose(3); - Eigen::VectorXd desiredLHandConf(L_ARM), desiredRHandConf(R_ARM); - - //desiredHeadPose << 53.9, 0.0, 194.4; // zero-position HEAD - desiredHeadPose << 0.0, 100.0, 0.0; - -// desiredLHandPose << 218.7, 133, 112.31; // zero-position L ARM - desiredLHandPose << -15.0, 316.0, 112.31; // point-left L ARM -// desiredLHandPose << 0, -100, -100; // point L ARM -// desiredLHandPose << -15.0, 98.0, 329.01; - desiredLHandConf = Eigen::VectorXd::Zero(desiredLHandConf.size()); - desiredLHandConf(1) = M_PI/2; -// desiredLHandConf(3) = -M_PI/2; - -// desiredLHandPose << 250, -250, 250; // +X -// desiredLHandPose << 0.0, 250.0, 0.0; // -Y -// desiredLHandPose << 0.0, 0.0, 250; // +Z - - -// desiredRHandPose << 218.7, -133, 112.31; //zero-position R ARM - desiredRHandPose << 15.0, -316.0, 112.31; //point-right R ARM -// desiredRHandPose << 0, -100, -100; // point R ARM -// desiredRHandPose << -15.0, -98.0, 329.01; - desiredRHandConf = Eigen::VectorXd::Zero(desiredRHandConf.size()); - desiredRHandConf(1) = -M_PI/2; -// desiredRHandConf(3) = M_PI/2; - -// desiredRHandPose << 250, -250, -250; // +X -// desiredRHandPose << 0.0, -250.0, 0.0; // -Y -// desiredRHandPose << 0.0, 0.0, 250; // +Z - - taskMap["Head task"]->setDesiredPose(desiredHeadPose, 1, Rmath::hTranslation(Eigen::Vector3d::Zero())); -// taskMap["Left arm task"]->setDesiredPose(desiredLHandPose, 15, Rmath::homX(-M_PI/2, CoM_LA) ); - taskMap["Left arm task"]->setDesiredConfiguration(desiredLHandConf, 3); -// taskMap["Right arm task"]->setDesiredPose(desiredRHandPose, 15, Rmath::homX(-M_PI/2, CoM_RA) ); - taskMap["Right arm task"]->setDesiredConfiguration(desiredRHandConf, 3); + Eigen::VectorXd desiredHeadPose(HEAD_TASK_DIM), desiredLHandPose(LARM_TASK_DIM), desiredRHandPose(RARM_TASK_DIM); + Eigen::VectorXd desiredHeadPosition(3), desiredRHandPosition(3), desiredLHandPosition(3); + Eigen::VectorXd desiredHeadOrientation(3), desiredRHandOrientation(3), desiredLHandOrientation(3); + + // head position in 3D space + desiredHeadPosition << HEAD_DESIRED_X, HEAD_DESIRED_Y, HEAD_DESIRED_Z; + // head orientation in 3D space + desiredHeadOrientation << HEAD_DESIRED_ROLL, HEAD_DESIRED_PITCH, HEAD_DESIRED_YAW; + // Right arm position in 3D space + desiredRHandPosition << RARM_DESIRED_X, RARM_DESIRED_Y, RARM_DESIRED_Z; + // Right arm orientation in 3D space + desiredRHandOrientation << RARM_DESIRED_ROLL, RARM_DESIRED_PITCH, RARM_DESIRED_YAW; + // left arm position in 3D space + desiredLHandPosition << LARM_DESIRED_X, LARM_DESIRED_Y, LARM_DESIRED_Z; + // left arm orientation in 3D space + desiredLHandOrientation << LARM_DESIRED_ROLL, LARM_DESIRED_PITCH, LARM_DESIRED_YAW; + + if( HEAD_TASK_DIM > 3 ) + desiredHeadPose << desiredHeadPosition, desiredHeadOrientation.head(HEAD_TASK_DIM-3); + else + desiredHeadPose << desiredHeadPosition.head(HEAD_TASK_DIM); + + if( LARM_TASK_DIM > 3 ) + desiredLHandPose << desiredLHandPosition, desiredLHandOrientation.head(LARM_TASK_DIM-3); + else + desiredLHandPose << desiredLHandPosition.head(LARM_TASK_DIM); + + if( RARM_TASK_DIM > 3 ) + desiredRHandPose << desiredRHandPosition, desiredRHandOrientation.head(RARM_TASK_DIM-3); + else + desiredRHandPose << desiredRHandPosition.head(RARM_TASK_DIM); + +// taskMap["Head task"]->setDesiredConfiguration(desiredHeadPose.head(HEAD_TASK_DIM), HEAD_TASK_NSTEPS); + taskMap["Head task"]->setDesiredPose(desiredHeadPose.head(HEAD_TASK_DIM), HEAD_TASK_NSTEPS, + Rmath::hTranslation(Eigen::Vector3d::Zero())); + + taskMap["Left arm task"]->setDesiredPose(desiredLHandPose.head(LARM_TASK_DIM), LARM_TASK_NSTEPS, + Rmath::homX(-M_PI/2, CoM_LA) ); +// taskMap["Left arm task"]->circularPathGenerator(desiredLHandPose.head(LARM_TASK_DIM), -70, LARM_TASK_NSTEPS, 100, 3, +// Rmath::homX(-M_PI/2, CoM_LA)); + +// taskMap["Right arm task"]->setDesiredPose(desiredRHandPose.head(RARM_TASK_DIM), RARM_TASK_NSTEPS, +// Rmath::homX(-M_PI/2, CoM_RA) ); + taskMap["Right arm task"]->circularPathGenerator(desiredRHandPose.head(RARM_TASK_DIM), -70, RARM_TASK_NSTEPS, 100, 3, + Rmath::homX(-M_PI/2, CoM_RA)); #ifdef DEBUG_MODE INFO("Desired head position in space: [\n" << desiredHeadPose << "]"); @@ -407,6 +393,8 @@ void R2Module::motion() taskMap["Left arm task"]->activate(); taskMap["Right arm task"]->activate(); + closeHand("RHand"); + openHand("LHand"); // Main loop while(true) { @@ -419,7 +407,6 @@ void R2Module::motion() for(unsigned int i=0; i(HEAD + R_ARM /*+ 1*/) = J_TMP_pinv * r; + qdot.segment(LARM_CHAIN_BEGIN) = J_TMP_pinv * r; INFO("Solution: [\n" << qdot << "]"); #endif @@ -534,9 +521,9 @@ void R2Module::updateConstraints(const Eigen::VectorXd& q) { // Updating the base kinematic chain (left foot to body center) Eigen::Matrix4d H_base; - Eigen::MatrixXd J_base(6, L_LEG); + Eigen::MatrixXd J_base(6, LLEG_CHAIN_SIZE); // Left leg joints are those on the bottom part of the IDs vector - theKinChainLeftLeg->update(q.tail(L_LEG)); + theKinChainLeftLeg->update(q.tail(LLEG_CHAIN_SIZE)); // Compute kinematics theKinChainLeftLeg->forward(&H_base); theKinChainLeftLeg->differential(&J_base); @@ -554,10 +541,10 @@ void R2Module::updateConstraints(const Eigen::VectorXd& q) #ifdef TEST_KINCHAIN INFO("Updating test kinematic chain (body to left hand): "); - q_TMP << q.segment(HEAD + R_ARM); + q_TMP << q.segment(LARM_CHAIN_BEGIN); INFO("Joint configuration:"); for(unsigned int i=0; iupdate(q_TMP); theKinChain_TMP->forward(&H_TMP); @@ -578,9 +565,9 @@ void R2Module::updateConstraints(const Eigen::VectorXd& q) Eigen::Vector3d CoM_RA; CoM_RA << 0.0, -98.0, 100.0; - // Head task update - ( taskMap["Head task"] )->update( q.head(HEAD), Eigen::VectorXd::Zero(3), K_HEAD, Rmath::hTranslation(Eigen::Vector3d::Zero())); + ( taskMap["Head task"] )->update( q.head(HEAD_CHAIN_SIZE), Eigen::VectorXd::Zero(HEAD_TASK_DIM), K_HEAD, + Rmath::hTranslation(Eigen::Vector3d::Zero())); #ifdef DEBUG_MODE INFO("Head task constraint equation: "); @@ -588,8 +575,9 @@ void R2Module::updateConstraints(const Eigen::VectorXd& q) #endif // Left arm task update -// ( taskMap["Left arm task"] )->update( q.segment(HEAD+R_ARM), Eigen::VectorXd::Zero(3), K_HEAD, Rmath::homX(-M_PI/2, CoM_LA) ); - ( taskMap["Left arm task"] )->update( q.segment(HEAD+R_ARM), Eigen::VectorXd::Zero(L_ARM), K_HEAD); + ( taskMap["Left arm task"] )->update( q.segment(LARM_CHAIN_BEGIN), Eigen::VectorXd::Zero(LARM_TASK_DIM), + K_LARM, Rmath::homX(-M_PI/2, CoM_LA) ); +// ( taskMap["Left arm task"] )->update( q.segment(LARM_CHAIN_BEGIN), Eigen::VectorXd::Zero(LARM_CHAIN_SIZE), K_LARM); #ifdef DEBUG_MODE INFO("Left arm task constraint equation: "); @@ -597,8 +585,9 @@ void R2Module::updateConstraints(const Eigen::VectorXd& q) #endif // Right arm task update -// ( taskMap["Right arm task"] )->update( q.segment(HEAD), Eigen::VectorXd::Zero(3), K_HEAD, Rmath::homX(-M_PI/2, CoM_RA) ); - ( taskMap["Right arm task"] )->update( q.segment(HEAD), Eigen::VectorXd::Zero(R_ARM), K_HEAD); + ( taskMap["Right arm task"] )->update( q.segment(RARM_CHAIN_BEGIN), Eigen::VectorXd::Zero(RARM_TASK_DIM), + K_RARM, Rmath::homX(-M_PI/2, CoM_RA) ); +// ( taskMap["Right arm task"] )->update( q.segment(RARM_CHAIN_BEGIN), Eigen::VectorXd::Zero(RARM_CHAIN_SIZE), K_RARM); #ifdef DEBUG_MODE INFO("Right arm task constraint equation: "); @@ -683,5 +672,54 @@ void R2Module::posBound2velBound( const Eigen::MatrixXd& posBound, const Eigen:: *velBound = (posBound - currConf)/TIME_STEP; } +void R2Module::closeHand(const std::string& handID) +{ + if(handID == "both") + { + fMotionProxy->setStiffnesses("LHand", 1.0); + fMotionProxy->setStiffnesses("RHand", 1.0); + fMotionProxy->closeHand("LHand"); + fMotionProxy->closeHand("RHand"); + fMotionProxy->setStiffnesses("LHand", 0.0); + fMotionProxy->setStiffnesses("RHand", 0.0); + } + else + { + fMotionProxy->setStiffnesses(handID, 1.0); + fMotionProxy->closeHand(handID); + fMotionProxy->setStiffnesses(handID, 0.0); + } + +} + +void R2Module::openHand(const std::string& handID) +{ + if(handID == "both") + { + fMotionProxy->setStiffnesses("LHand", 1.0); + fMotionProxy->setStiffnesses("RHand", 1.0); + fMotionProxy->openHand("LHand"); + fMotionProxy->openHand("RHand"); + fMotionProxy->setStiffnesses("LHand", 0.0); + fMotionProxy->setStiffnesses("RHand", 0.0); + } + else + { + fMotionProxy->setStiffnesses(handID, 1.0); + fMotionProxy->openHand(handID); + fMotionProxy->setStiffnesses(handID, 0.0); + } + +} + +Eigen::Vector3d R2Module::getRedBallPosition() +{ + Eigen::Vector3d ballPosition; + ballPosition << fBallTrackerProxy->getPosition().at(0), + fBallTrackerProxy->getPosition().at(1), + fBallTrackerProxy->getPosition().at(2); + + return ballPosition; +} } // namespace AL diff --git a/naoqi/modules/R2module/src/task.cpp b/naoqi/modules/R2module/src/task.cpp index 8983f9f..1655162 100644 --- a/naoqi/modules/R2module/src/task.cpp +++ b/naoqi/modules/R2module/src/task.cpp @@ -223,9 +223,7 @@ TaskBase TaskBase::operator--(int) return tmp_tb; } - -/** ---------- TaskBase ---------- */ - +/** ---------- Task ---------- */ /* * Class constructor. Input arguments are: @@ -255,7 +253,6 @@ Task::~Task() if(theKinChain) delete theKinChain; } - Eigen::VectorXd Task::getCurrentPose(const Eigen::Matrix4d& baseTransform) const { Eigen::Matrix4d H_chain; @@ -274,7 +271,6 @@ Eigen::VectorXd Task::getCurrentPose(const Eigen::Matrix4d& baseTransform) const return currentPose; } - void Task::setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints, const Eigen::Matrix4d& baseTransform) { // Re-computing direct kinematics @@ -318,11 +314,53 @@ void Task::setDesiredConfiguration(const Eigen::VectorXd& desiredConf, int n_con if (!jointControlActive) jointControlActive = true; } +void Task::circularPathGenerator( const Eigen::VectorXd& dp, float z_shift, int n_controlPoints, float radius, int n, + const Eigen::Matrix4d& baseTransform ) +{ + // Re-computing direct kinematics + Eigen::Matrix4d H_chain; + theKinChain->forward((&H_chain)); + H_chain = baseTransform * H_chain; + +// Eigen::Vector3d initialTranslation; +// initialTranslation << 218.7, 113, 112.31; +// Eigen::Vector3d transEEd = dp.head(3) - initialTranslation; +// INFO("desired vector: \n" << dp.head(3)); +// INFO("current vector: \n" << H_chain.topRightCorner(3,1)); +// /* very beginning Nao left hand translation +// 218.7, 113, 112.31 +// */ +// INFO("translation vector: \n" << transEEd); + + for (float i = 0.0; i < n*2*M_PI; i+= 2*M_PI/n_controlPoints) + { + Eigen::VectorXd ee_desiredPose_handframe(4); + ee_desiredPose_handframe << /*transEEd(0)+*/ radius*cos(i), + /*transEEd(1)+*/ radius*sin(i), + /*transEEd(2)+*/ z_shift, + 1.0; + Rmath::trim(&ee_desiredPose_handframe); + + Eigen::VectorXd ee_desiredPose_CoMframe(dp.size()); + if(dp.size() > 3) + ee_desiredPose_CoMframe << (H_chain * ee_desiredPose_handframe).head(3), dp(3), dp(4), dp(5); + else + ee_desiredPose_CoMframe << (H_chain * ee_desiredPose_handframe).head(3); + + Rmath::trim(&ee_desiredPose_CoMframe); + + path.push_back( ee_desiredPose_CoMframe ); + } + + path_currentStep = 0; + positioningActive = true; + if (jointControlActive) jointControlActive = false; +} + /* * Task update function: update the constraint matrix A=A(q) with a new value of q and replace the target vector * with a proportional/derivative control law of the type K*POSE_ERROR + DESIRED_VELOCITY. */ - void Task::update( const Eigen::VectorXd& q, const Eigen::VectorXd& desiredVel, double K, const Eigen::Matrix4d& baseTransform ) { // Dimensions must be consistent From a17ec3d47418642154c078b9d11bdfa5f11a4a70 Mon Sep 17 00:00:00 2001 From: Francesco Riccio Date: Thu, 12 Dec 2013 23:30:01 +0100 Subject: [PATCH 4/6] R2::module updated --- naoqi/modules/R2module/config/CoM_head.cfg | 4 + naoqi/modules/R2module/config/CoM_leftArm.cfg | 5 + naoqi/modules/R2module/config/CoM_leftLeg.cfg | 9 + .../modules/R2module/config/CoM_rightArm.cfg | 5 + .../modules/R2module/config/CoM_rightLeg.cfg | 9 + naoqi/modules/R2module/config/dh_LAbase.cfg | 8 + naoqi/modules/R2module/config/dh_LLbase.cfg | 17 + naoqi/modules/R2module/config/dh_RAbase.cfg | 8 + naoqi/modules/R2module/config/dh_RLbase.cfg | 15 + naoqi/modules/R2module/config/dh_head.cfg | 8 +- naoqi/modules/R2module/config/dh_leftArm.cfg | 10 +- naoqi/modules/R2module/config/dh_leftLeg.cfg | 29 +- naoqi/modules/R2module/config/dh_rightArm.cfg | 10 +- naoqi/modules/R2module/config/dh_rightLeg.cfg | 29 +- .../modules/R2module/config/joints_params.cfg | 1 + naoqi/modules/R2module/include/configParams.h | 214 +++-- naoqi/modules/R2module/include/configReader.h | 16 +- .../modules/R2module/include/libmath/Rutils.h | 6 +- .../R2module/include/libmath/kinChain.h | 15 +- naoqi/modules/R2module/include/r2Module.h | 20 +- naoqi/modules/R2module/include/task.h | 30 +- naoqi/modules/R2module/src/configReader.cpp | 76 +- .../modules/R2module/src/libmath/kinChain.cpp | 148 ++-- naoqi/modules/R2module/src/r2Module.cpp | 367 +++++--- .../modules/R2module/src/r2Module_backUp.cpp | 834 ++++++++++++++++++ naoqi/modules/R2module/src/task.cpp | 131 ++- 26 files changed, 1642 insertions(+), 382 deletions(-) create mode 100644 naoqi/modules/R2module/config/CoM_head.cfg create mode 100644 naoqi/modules/R2module/config/CoM_leftArm.cfg create mode 100644 naoqi/modules/R2module/config/CoM_leftLeg.cfg create mode 100644 naoqi/modules/R2module/config/CoM_rightArm.cfg create mode 100644 naoqi/modules/R2module/config/CoM_rightLeg.cfg create mode 100644 naoqi/modules/R2module/config/dh_LAbase.cfg create mode 100644 naoqi/modules/R2module/config/dh_LLbase.cfg create mode 100644 naoqi/modules/R2module/config/dh_RAbase.cfg create mode 100644 naoqi/modules/R2module/config/dh_RLbase.cfg create mode 100644 naoqi/modules/R2module/src/r2Module_backUp.cpp diff --git a/naoqi/modules/R2module/config/CoM_head.cfg b/naoqi/modules/R2module/config/CoM_head.cfg new file mode 100644 index 0000000..69411ea --- /dev/null +++ b/naoqi/modules/R2module/config/CoM_head.cfg @@ -0,0 +1,4 @@ +[ T ] = +{ +T 0 0 126.5 +} diff --git a/naoqi/modules/R2module/config/CoM_leftArm.cfg b/naoqi/modules/R2module/config/CoM_leftArm.cfg new file mode 100644 index 0000000..87d79c9 --- /dev/null +++ b/naoqi/modules/R2module/config/CoM_leftArm.cfg @@ -0,0 +1,5 @@ +[ T ] = +{ +T 0 98 100 +R 1 0 0 -M_PI/2 +} diff --git a/naoqi/modules/R2module/config/CoM_leftLeg.cfg b/naoqi/modules/R2module/config/CoM_leftLeg.cfg new file mode 100644 index 0000000..9591dab --- /dev/null +++ b/naoqi/modules/R2module/config/CoM_leftLeg.cfg @@ -0,0 +1,9 @@ +[ T ] = +{ +T 0 50 -85 +R 1 0 0 M_PI/4 +R 1 0 0 M_PI +} +H 0 0 0 q1 : LHipYawPitch +R 1 0 0 -M_PI/4 +R 0 1 0 -M_PI/2 diff --git a/naoqi/modules/R2module/config/CoM_rightArm.cfg b/naoqi/modules/R2module/config/CoM_rightArm.cfg new file mode 100644 index 0000000..66633ed --- /dev/null +++ b/naoqi/modules/R2module/config/CoM_rightArm.cfg @@ -0,0 +1,5 @@ +[ T ] = +{ +T 0 -98 100 +R 1 0 0 -M_PI/2 +} diff --git a/naoqi/modules/R2module/config/CoM_rightLeg.cfg b/naoqi/modules/R2module/config/CoM_rightLeg.cfg new file mode 100644 index 0000000..17cb4cc --- /dev/null +++ b/naoqi/modules/R2module/config/CoM_rightLeg.cfg @@ -0,0 +1,9 @@ +[ T ] = +{ +T 0 -50 -85 +R 1 0 0 -M_PI/4 +} +H 0 0 0 q1 : RHipYawPitch +R 1 0 0 -M_PI/4 +R 1 0 0 -M_PI/2 +R 0 1 0 M_PI/2 diff --git a/naoqi/modules/R2module/config/dh_LAbase.cfg b/naoqi/modules/R2module/config/dh_LAbase.cfg new file mode 100644 index 0000000..a468b20 --- /dev/null +++ b/naoqi/modules/R2module/config/dh_LAbase.cfg @@ -0,0 +1,8 @@ +[ chain ] = +{ +: LWristYaw +: LElbowRoll +: LElbowYaw +: LShoulderRoll +: LShoulderPitch +} diff --git a/naoqi/modules/R2module/config/dh_LLbase.cfg b/naoqi/modules/R2module/config/dh_LLbase.cfg new file mode 100644 index 0000000..40be273 --- /dev/null +++ b/naoqi/modules/R2module/config/dh_LLbase.cfg @@ -0,0 +1,17 @@ +[ chain ] = +{ +H 0 M_PI/2 0 q1 : LAnkleRoll +H 102.9 0 0 q2 : LAnklePitch +H 100 0 0 q3 : LKneePitch +H 0 -M_PI/2 0 q4 : LHipPitch +H 0 0 0 q5 : LHipRoll +R 1 0 0 M_PI/2 +R 0 1 0 M_PI/4 +H 0 0 0 q6 : LHipYawPitch +R 0 1 0 M_PI/4 +R 0 0 1 M_PI/2 +T 0 -50 85 +} + + + diff --git a/naoqi/modules/R2module/config/dh_RAbase.cfg b/naoqi/modules/R2module/config/dh_RAbase.cfg new file mode 100644 index 0000000..e8f45ff --- /dev/null +++ b/naoqi/modules/R2module/config/dh_RAbase.cfg @@ -0,0 +1,8 @@ +[ chain ] = +{ +: RWristYaw +: RElbowRoll +: RElbowYaw +: RShoulderRoll +: RShoulderPitch +} diff --git a/naoqi/modules/R2module/config/dh_RLbase.cfg b/naoqi/modules/R2module/config/dh_RLbase.cfg new file mode 100644 index 0000000..f19436e --- /dev/null +++ b/naoqi/modules/R2module/config/dh_RLbase.cfg @@ -0,0 +1,15 @@ +[ chain ] = +{ +H 0 M_PI/2 0 q1 : RAnkleRoll +H 102.9 0 0 q2 : RAnklePitch +H 100 0 0 q3 : RKneePitch +H 0 -M_PI/2 0 q4 : RHipPitch +H 0 0 0 q5 : RHipRoll +R 1 0 0 M_PI/2 +H 0 0 0 q6 : RHipYawPitch +R 0 1 0 M_PI/4 +R 0 1 0 M_PI/4 +R 0 0 1 M_PI/2 +T 0 50 85 +} + diff --git a/naoqi/modules/R2module/config/dh_head.cfg b/naoqi/modules/R2module/config/dh_head.cfg index 7c2cc03..d3e31c6 100644 --- a/naoqi/modules/R2module/config/dh_head.cfg +++ b/naoqi/modules/R2module/config/dh_head.cfg @@ -1,8 +1,6 @@ [ chain ] = { -H 0 -M_PI/2 0 q1 -H 0 M_PI/2 0 q2 -T 53.9 0 67.9 +H 0 -M_PI/2 0 q1 : HeadYaw +H 0 M_PI/2 0 q2 : HeadPitch } -# CoM-head shift -T 0 0 126.5 + diff --git a/naoqi/modules/R2module/config/dh_leftArm.cfg b/naoqi/modules/R2module/config/dh_leftArm.cfg index 8a514cf..42e84d2 100644 --- a/naoqi/modules/R2module/config/dh_leftArm.cfg +++ b/naoqi/modules/R2module/config/dh_leftArm.cfg @@ -1,14 +1,14 @@ [ chain ] = { -H 0 M_PI/2 0 q1 -H 0 0 0 q2 +H 0 M_PI/2 0 q1 : LShoulderPitch +H 0 0 0 q2 : LShoulderRoll R 0 0 1 -M_PI/2 R 1 0 0 -M_PI/2 -H -15 M_PI/2 105 q3 +H -15 M_PI/2 105 q3 : LElbowYaw R 0 0 1 M_PI/2 -H 0 0 0 q4 +H 0 0 0 q4 : LElbowRoll T 113.7 0 12.31 R 0 0 1 -M_PI/2 R 1 0 0 -M_PI/2 -H 0 0 0 q5 +H 0 0 0 q5 : LWristYaw } diff --git a/naoqi/modules/R2module/config/dh_leftLeg.cfg b/naoqi/modules/R2module/config/dh_leftLeg.cfg index f07dd24..28a6306 100644 --- a/naoqi/modules/R2module/config/dh_leftLeg.cfg +++ b/naoqi/modules/R2module/config/dh_leftLeg.cfg @@ -1,18 +1,23 @@ [ chain ] = { +H 0 0 0 q1 : LHipYawPitch +R 1 0 0 -M_PI/4 R 0 1 0 M_PI/2 -R 0 0 1 M_PI -T 45.19 0 0 -H 0 M_PI/2 0 q1 -H 102.9 0 0 q2 -H 100 -M_PI/2 0 q3 -H 0 M_PI/2 0 q4 -H 0 0 0 q5 -R 0 1 0 -M_PI/4 -H 0 0 0 q6 -R 0 1 0 M_PI/4 + +H 0 0 0 q1 : LHipRoll +R 1 0 0 M_PI/2 +R 0 0 1 M_PI/2 +H 0 0 0 q2 : LHipPitch +T 0 100 0 +H 0 0 0 q3 : LKneePitch +T 0 102.9 0 +H 0 0 0 q4 : LAnklePitch +R 1 0 0 M_PI/2 R 0 0 1 M_PI/2 -T 0 0 -50 R 1 0 0 M_PI/2 -T 0 0 85 +H 0 0 0 q5 : LAnkleRoll +T 0 -45.19 0 +R 0 0 1 M_PI/2 } + + diff --git a/naoqi/modules/R2module/config/dh_rightArm.cfg b/naoqi/modules/R2module/config/dh_rightArm.cfg index 06d2d09..84f317e 100644 --- a/naoqi/modules/R2module/config/dh_rightArm.cfg +++ b/naoqi/modules/R2module/config/dh_rightArm.cfg @@ -1,14 +1,14 @@ [ chain ] = { -H 0 M_PI/2 0 q1 -H 0 0 0 q2 +H 0 M_PI/2 0 q1 : RShoulderPitch +H 0 0 0 q2 : RShoulderRoll R 0 0 1 -M_PI/2 R 1 0 0 -M_PI/2 -H 15 M_PI/2 105 q3 +H 15 M_PI/2 105 q3 : RElbowYaw R 0 0 1 M_PI/2 -H 0 0 0 q4 +H 0 0 0 q4 : RElbowRoll T 113.7 0 12.31 R 0 0 1 -M_PI/2 R 1 0 0 -M_PI/2 -H 0 0 0 q5 +H 0 0 0 q5 : RWristYaw } diff --git a/naoqi/modules/R2module/config/dh_rightLeg.cfg b/naoqi/modules/R2module/config/dh_rightLeg.cfg index c5d8a7b..8d3891c 100644 --- a/naoqi/modules/R2module/config/dh_rightLeg.cfg +++ b/naoqi/modules/R2module/config/dh_rightLeg.cfg @@ -1,14 +1,23 @@ [ chain ] = { -T 0 -50 -85 +H 0 0 0 q1 : RHipYawPitch R 1 0 0 -M_PI/4 -H 0 0 0 q1 -R 1 0 0 -M_PI/4 -H 0 -M_PI/2 0 q2 -H -100 M_PI/2 0 q3 -H -102.9 0 0 q4 -H 0 -M_PI/2 0 q5 -H -45.19 0 0 q6 -R 0 0 1 -M_PI -R 0 1 0 -M_PI/2 +R 1 0 0 -M_PI/2 +R 0 1 0 M_PI/2 + +H 0 0 0 q1 : RHipRoll +R 1 0 0 M_PI/2 +R 0 0 1 M_PI/2 +H 0 0 0 q2 : RHipPitch +T 0 100 0 +H 0 0 0 q3 : RKneePitch +T 0 102.9 0 +H 0 0 0 q4 : RAnklePitch +R 1 0 0 M_PI/2 +R 0 0 1 M_PI/2 +R 1 0 0 M_PI/2 +H 0 0 0 q5 : RAnkleRoll +T 0 -45.19 0 +R 0 0 1 M_PI/2 } + diff --git a/naoqi/modules/R2module/config/joints_params.cfg b/naoqi/modules/R2module/config/joints_params.cfg index 0e982b9..37df5cf 100644 --- a/naoqi/modules/R2module/config/joints_params.cfg +++ b/naoqi/modules/R2module/config/joints_params.cfg @@ -25,3 +25,4 @@ 0.0 -1.189516 0.922747 LAnklePitch 0.0 -0.397880 0.769001 LAnkleRoll } + diff --git a/naoqi/modules/R2module/include/configParams.h b/naoqi/modules/R2module/include/configParams.h index ee11c96..50afb37 100644 --- a/naoqi/modules/R2module/include/configParams.h +++ b/naoqi/modules/R2module/include/configParams.h @@ -3,67 +3,124 @@ #include -// CIRCLE:: n°circles, z_shift, radius +#define DEBUG_MODE -// Nao joints number -static const int JOINTS_NUM = 24; // Discrete integration time step static const double TIME_STEP = 1.0; + +// Nao numerical values +#define FOOT_HEIGHT 45.19 + +#define HIP_HEIGHT 202.9 +#define HIP_PELVIS_SHIFT 50.0 +#define HIP_TORSO 85.0 + +#define TORSO_NECK 126.5 +#define TORSO_SHOULDER_Z 100.0 +#define TORSO_SHOULDER_Y 98.0 + +#define ARM_LENGTH 218.0 +#define WRIST_SHIFT_Z 12.31 +#define ELBOW_SHIFT_Y 15.0 + + +/** ---------------------------- Paths ---------------------------- */ + + // Config (.cfg) files directory and paths //static const std::string CONFIG_PATH = "/home/claudio/Naoqi/NaoModules/r2module/config/"; static const std::string CONFIG_PATH = "/home/francesco/naoqi/modules/R2module/config/"; +// Joint parameters static const std::string JOINT_BOUNDS_CFG = "joints_params.cfg"; +// Base kinematic chains +static const std::string LEFT_LEG_BASE_CFG = "dh_LLbase.cfg"; +static const std::string RIGHT_LEG_BASE_CFG = "dh_RLbase.cfg"; +// Kinematic chains static const std::string LEFT_LEG_CFG = "dh_leftLeg.cfg"; static const std::string LEFT_ARM_CFG = "dh_leftArm.cfg"; static const std::string RIGHT_LEG_CFG = "dh_rightLeg.cfg"; static const std::string RIGHT_ARM_CFG = "dh_rightArm.cfg"; static const std::string HEAD_CFG = "dh_head.cfg"; +// Fixed transformations +static const std::string COM_HEAD_CFG = "CoM_head.cfg"; +static const std::string COM_RIGHT_ARM_CFG = "CoM_rightArm.cfg"; +static const std::string COM_LEFT_ARM_CFG = "CoM_leftArm.cfg"; +static const std::string COM_RIGHT_LEG_CFG = "CoM_rightLeg.cfg"; +static const std::string COM_LEFT_LEG_CFG = "CoM_leftLeg.cfg"; -/** ---------------------- Head task parameters ---------------------- */ +/** ---------------------------- Joint parameters ---------------------------- */ + + +// Head static const int HEAD_CHAIN_BEGIN = 0; static const int HEAD_CHAIN_SIZE = 2; static const int HEAD_CHAIN_END = HEAD_CHAIN_BEGIN + HEAD_CHAIN_SIZE; +// Right arm +static const int RARM_CHAIN_BEGIN = HEAD_CHAIN_END; +static const int RARM_CHAIN_SIZE = 5; +static const int RARM_CHAIN_END = RARM_CHAIN_BEGIN + RARM_CHAIN_SIZE; +// Left arm +static const int LARM_CHAIN_BEGIN = RARM_CHAIN_END; +static const int LARM_CHAIN_SIZE = 5; +static const int LARM_CHAIN_END = LARM_CHAIN_BEGIN + LARM_CHAIN_SIZE; +// Right leg +static const int RLEG_CHAIN_BEGIN = LARM_CHAIN_END; +static const int RLEG_CHAIN_SIZE = 6; +static const int RLEG_CHAIN_END = RLEG_CHAIN_BEGIN + RLEG_CHAIN_SIZE; +// Left leg +static const int LLEG_CHAIN_BEGIN = RLEG_CHAIN_END; +static const int LLEG_CHAIN_SIZE = 6; +static const int LLEG_CHAIN_END = LLEG_CHAIN_BEGIN + LLEG_CHAIN_SIZE; -static const int HEAD_TASK_DIM = 3; -static const int HEAD_TASK_PRIORITY = 3; -static const int K_HEAD = 1; -static const int HEAD_TASK_NSTEPS = 1; +// Nao joints number +static const int JOINTS_NUM = HEAD_CHAIN_SIZE + + LARM_CHAIN_SIZE + RARM_CHAIN_SIZE + + LLEG_CHAIN_SIZE + RLEG_CHAIN_SIZE; -static const double HEAD_DESIRED_X = 53.9; -static const double HEAD_DESIRED_Y = 0.0; -static const double HEAD_DESIRED_Z = 194.4; -static const double HEAD_DESIRED_ROLL = 0.0; -static const double HEAD_DESIRED_PITCH = 0.0; -static const double HEAD_DESIRED_YAW = 0.0; +// Fixed transformation base-ankle frame +static const double BASE_ANKLE_X = 0.0; +static const double BASE_ANKLE_Y = 0.0; +static const double BASE_ANKLE_Z = FOOT_HEIGHT; +static Eigen::Matrix4d base_ankle; -/** -------------------- Right arm task parameters -------------------- */ +/** ---------------------------- Task parameters ---------------------------- */ -static const int RARM_CHAIN_BEGIN = HEAD_CHAIN_END; -static const int RARM_CHAIN_SIZE = 5; -static const int RARM_CHAIN_END = RARM_CHAIN_BEGIN + RARM_CHAIN_SIZE; -static const double COM_RARM_SHIFT_X = 0.0; -static const double COM_RARM_SHIFT_Y = -98.0; -static const double COM_RARM_SHIFT_Z = 100.0; +// Task dimensions +static const int HEAD_TASK_DIM = 6; static const int RARM_TASK_DIM = 3; -static const int RARM_TASK_PRIORITY = 1; -static const int K_RARM = 1; +static const int LARM_TASK_DIM = 3; +static const int RLEG_TASK_DIM = 6; +static const int LLEG_TASK_DIM = 6; + +// Task priorities +static const int HEAD_TASK_PRIORITY = 5; +static const int RARM_TASK_PRIORITY = 3; +static const int LARM_TASK_PRIORITY = 2; +static const int RLEG_TASK_PRIORITY = 1; +static const int LLEG_TASK_PRIORITY = 1; + +// Gains +static const double K_HEAD = 1.0; +static const double K_RARM = 1.0; +static const double K_LARM = 1.0; +static const double K_RLEG = 0.8; +static const double K_LLEG = 0.8; + +// Control points number +static const int HEAD_TASK_NSTEPS = 1; static const int RARM_TASK_NSTEPS = 30; - -static const double RARM_DESIRED_X = 250.0; -static const double RARM_DESIRED_Y = -113.0; -static const double RARM_DESIRED_Z = 120.0; -static const double RARM_DESIRED_ROLL = 0.0; -static const double RARM_DESIRED_PITCH = 0.0; -static const double RARM_DESIRED_YAW = 0.0; +static const int LARM_TASK_NSTEPS = 30; +static const int RLEG_TASK_NSTEPS = 30; +static const int LLEG_TASK_NSTEPS = 30; #define RARM_LARM_JOINT_TASK @@ -71,58 +128,69 @@ enum TaskType { MIRROR_TASK, MIMIC_TASK }; -static const TaskType ARMS_TASK = MIMIC_TASK; -static const double MINIMUM_HANDS_DISTANCE = 50.0; - -/** -------------------- Left arm task parameters -------------------- */ - - -static const int LARM_CHAIN_BEGIN = RARM_CHAIN_END; -static const int LARM_CHAIN_SIZE = 5; -static const int LARM_CHAIN_END = LARM_CHAIN_BEGIN + LARM_CHAIN_SIZE; +static const TaskType ARMS_TASK = MIRROR_TASK; +static const double MINIMUM_HANDS_DISTANCE = 350.0; -static const double COM_LARM_SHIFT_X = 0.0; -static const double COM_LARM_SHIFT_Y = 98.0; -static const double COM_LARM_SHIFT_Z = 100.0; +// Circle trajectory task parameters +//#define LARM_CIRCLE_TASK +//#define RARM_CIRCLE_TASK +static const double CIRCLE_Z_DEPTH = -70.0; // wrt ee-frame +static const double CIRCLE_RADIUS = 70.0; +static const int CIRCLE_LAPS = 10; -static const int LARM_TASK_DIM = 3; -static const int LARM_TASK_PRIORITY = 1; -static const int K_LARM = 1; -static const int LARM_TASK_NSTEPS = 30; - -static const double LARM_DESIRED_X = 0.0; -static const double LARM_DESIRED_Y = 250 + 113.0; -static const double LARM_DESIRED_Z = 120.0; -static const double LARM_DESIRED_ROLL = 0.0; -static const double LARM_DESIRED_PITCH = 0.0; -static const double LARM_DESIRED_YAW = M_PI/2; +// Up-down with legs task +//#define UP_DOWN_TASK +static int UP_DOWN = 80.0; -/** -------------------- Right leg task parameters -------------------- */ - -static const int RLEG_CHAIN_BEGIN = LARM_CHAIN_END; -static const int RLEG_CHAIN_SIZE = 6; -static const int RLEG_CHAIN_END = RLEG_CHAIN_BEGIN + RLEG_CHAIN_SIZE; +/** ------------------------------- Desired poses ------------------------------- */ -/** -------------------- Left leg task parameters -------------------- */ - -static const int LLEG_CHAIN_BEGIN = RLEG_CHAIN_END; -static const int LLEG_CHAIN_SIZE = 6; -static const int LLEG_CHAIN_END = LLEG_CHAIN_BEGIN + LLEG_CHAIN_SIZE; - -static const int LLEG_TASK_DIM = 3; - - -#ifdef TEST_KINCHAIN -static Rmath::KinChain* theKinChain_TMP; -static Eigen::Matrix4d H_TMP; -static Eigen::MatrixXd J_TMP; -static Eigen::VectorXd q_TMP(LARM_CHAIN_SIZE); -static Eigen::Vector3d r; +//Head +static const double HEAD_DESIRED_X = 0.0; // [mm] +static const double HEAD_DESIRED_Y = 0.0; +static const double HEAD_DESIRED_Z = FOOT_HEIGHT+HIP_HEIGHT+HIP_TORSO+TORSO_NECK-9.9; +static const double HEAD_DESIRED_ROLL = 0.0; +static const double HEAD_DESIRED_PITCH = 0.0; +static const double HEAD_DESIRED_YAW = 0.0; +//Right arm +static const double RARM_DESIRED_X = ARM_LENGTH; +static const double RARM_DESIRED_Y = -HIP_PELVIS_SHIFT-TORSO_SHOULDER_Y-ELBOW_SHIFT_Y; +static const double RARM_DESIRED_Z = FOOT_HEIGHT+HIP_HEIGHT+HIP_TORSO+TORSO_SHOULDER_Z+WRIST_SHIFT_Z-9.9; +static const double RARM_DESIRED_ROLL = 0.0; +static const double RARM_DESIRED_PITCH = 0.0; +static const double RARM_DESIRED_YAW = 0.0; +// Left arm +static const double LARM_DESIRED_X = ARM_LENGTH; +static const double LARM_DESIRED_Y = -HIP_PELVIS_SHIFT+TORSO_SHOULDER_Y+ELBOW_SHIFT_Y; +static const double LARM_DESIRED_Z = FOOT_HEIGHT+HIP_HEIGHT+HIP_TORSO+TORSO_SHOULDER_Z+WRIST_SHIFT_Z-9.9; +static const double LARM_DESIRED_ROLL = 0.0; +static const double LARM_DESIRED_PITCH = 0.0; +static const double LARM_DESIRED_YAW = M_PI_2; +// Right leg +static const double RLEG_DESIRED_X = 20.0; +static const double RLEG_DESIRED_Y = -HIP_PELVIS_SHIFT; +#ifndef UP_DOWN_TASK +static const double RLEG_DESIRED_Z = -HIP_HEIGHT-FOOT_HEIGHT-HIP_TORSO+9.9; +#else +static const double RLEG_DESIRED_Z = -HIP_HEIGHT-FOOT_HEIGHT-HIP_TORSO+UP_DOWN+9.9; +#endif +static const double RLEG_DESIRED_ROLL = 0.0; +static const double RLEG_DESIRED_PITCH = 0.0; +static const double RLEG_DESIRED_YAW = 0.0; +// Left leg +static const double LLEG_DESIRED_X = 20.0; +static const double LLEG_DESIRED_Y = HIP_PELVIS_SHIFT; +#ifndef UP_DOWN_TASK +static const double LLEG_DESIRED_Z = -HIP_HEIGHT-FOOT_HEIGHT-HIP_TORSO+9.9; +#else +static const double LLEG_DESIRED_Z = -HIP_HEIGHT-FOOT_HEIGHT-HIP_TORSO+UP_DOWN+9.9; #endif +static const double LLEG_DESIRED_ROLL = 0.0; +static const double LLEG_DESIRED_PITCH = 0.0; +static const double LLEG_DESIRED_YAW = 0.0; #endif diff --git a/naoqi/modules/R2module/include/configReader.h b/naoqi/modules/R2module/include/configReader.h index 3f51f88..31a789c 100644 --- a/naoqi/modules/R2module/include/configReader.h +++ b/naoqi/modules/R2module/include/configReader.h @@ -29,9 +29,9 @@ static double to_double(std::string s){ class ConfigReader{ private: std::vector transformations; - std::vector minBounds; - std::vector maxBounds; - std::vector zero_pose; + std::vector kinChainJointIDs; + + std::map > jointParams; std::vector jointID; void dhReader(std::string dh_file_path); @@ -39,11 +39,15 @@ class ConfigReader{ double valueReader(std::string _val); public: - ConfigReader(std::string dh_file_path, std::string params_file_path = ""); + ConfigReader(std::string params_file_path); + + void setKinChain(std::string dh_file_path); + + void storeJointsID(std::vector* _jointID); - void storeJointsID(std::vector *_jointID); + void kinChainJointsID(std::vector* _kcJointID); - void extractJointParams(std::vector* _min, std::vector* _max, std::vector* _zero_pose, int _i, int _j); + std::vector getJointParams(std::string _id); soth::VectorBound extractJointBounds(); void extractJointBounds(Eigen::MatrixXd* bounds); diff --git a/naoqi/modules/R2module/include/libmath/Rutils.h b/naoqi/modules/R2module/include/libmath/Rutils.h index e63821b..399cdf3 100644 --- a/naoqi/modules/R2module/include/libmath/Rutils.h +++ b/naoqi/modules/R2module/include/libmath/Rutils.h @@ -97,7 +97,7 @@ void homogenize(const Eigen::Matrix3d& m, const Eigen::Vector3d& v, Eigen::Matri * The result is stored in 'mx'. */ void Hx(double a, const Eigen::Vector3d& shift, Eigen::Matrix4d* mx); -Eigen::Matrix4d homX(double a, const Eigen::Vector3d& shift); +Eigen::Matrix4d homX(double a, const Eigen::Vector3d& shift = Eigen::Vector3d::Zero()); /* * Compute the 4x4 homogeneous transformation composed of: @@ -106,7 +106,7 @@ Eigen::Matrix4d homX(double a, const Eigen::Vector3d& shift); * The result is stored in 'mx'. */ void Hy(double a, const Eigen::Vector3d& shift, Eigen::Matrix4d* my); -Eigen::Matrix4d homY(double a, const Eigen::Vector3d& shift); +Eigen::Matrix4d homY(double a, const Eigen::Vector3d& shift = Eigen::Vector3d::Zero()); /* * Compute the 4x4 homogeneous transformation composed of: @@ -115,7 +115,7 @@ Eigen::Matrix4d homY(double a, const Eigen::Vector3d& shift); * The result is stored in 'mx'. */ void Hz(double a, const Eigen::Vector3d& shift, Eigen::Matrix4d* mz); -Eigen::Matrix4d homZ(double a, const Eigen::Vector3d& shift); +Eigen::Matrix4d homZ(double a, const Eigen::Vector3d& shift = Eigen::Vector3d::Zero()); Eigen::Matrix4d hTranslation(const Eigen::Vector3d& shift); diff --git a/naoqi/modules/R2module/include/libmath/kinChain.h b/naoqi/modules/R2module/include/libmath/kinChain.h index 209d635..b4fbdf8 100644 --- a/naoqi/modules/R2module/include/libmath/kinChain.h +++ b/naoqi/modules/R2module/include/libmath/kinChain.h @@ -39,23 +39,26 @@ class KinChain class Joint { public: + // Identifiers + std::string id; + int col; double value; double max; double min; - Joint(double _value = 0.0, double _max = Eigen::Infinity, double _min = -Eigen::Infinity): - value(_value), max(_max), min(_min) {} + Joint(std::string _id, int _col, double _value = 0.0, double _max = Eigen::Infinity, double _min = -Eigen::Infinity): + id(_id), col(_col), value(_value), max(_max), min(_min){} }; // Indices of the robot joints within the chain std::map joints; - public: // Constructors - KinChain(ConfigReader& theConfigReader, int _i, int _j); - KinChain(std::vector _transf); + KinChain(ConfigReader& theConfigReader); +// KinChain(ConfigReader& theConfigReader, int _i, int _j); + KinChain(std::vector _transf, std::map _joints); // Copy constructors KinChain(const Rmath::KinChain& kc); // Destructors @@ -66,6 +69,8 @@ class KinChain Eigen::VectorXd jointConfiguration(); // Getter for the joint limits const soth::VectorBound jointLimits(); + // Getter for joints names and indices + void getJointsIDs(std::map* jointsIDs) const; // Joint configuration update void update(int first, int last, Eigen::VectorXd &q); diff --git a/naoqi/modules/R2module/include/r2Module.h b/naoqi/modules/R2module/include/r2Module.h index eab505c..1c89ae1 100644 --- a/naoqi/modules/R2module/include/r2Module.h +++ b/naoqi/modules/R2module/include/r2Module.h @@ -74,14 +74,26 @@ class R2Module : public AL::ALModule // Joint limits primary task TaskBase* jointLimits; - // Common base kinematic chain (left foot to body center) - Rmath::KinChain* theKinChainLeftLeg; // All initialized tasks std::map taskMap; // Stack of tasks currently active std::set taskSet; - // Tasks respective positions within the whole kinematic chain - std::map taskLoc; + + // Common base kinematic chain (left foot to body center) + Rmath::KinChain* theKinChain_LLBase; + Rmath::KinChain* theKinChain_RLBase; + + Rmath::KinChain* theKinChainLeftLeg; + Rmath::KinChain* theKinChainRightLeg; + Rmath::KinChain* theKinChainLeftArm; + Rmath::KinChain* theKinChainRightArm; + Rmath::KinChain* theKinChainHead; + + Rmath::KinChain* CoM_LeftLeg; + Rmath::KinChain* CoM_RightLeg; + Rmath::KinChain* CoM_LeftArm; + Rmath::KinChain* CoM_RightArm; + Rmath::KinChain* CoM_Head; public: diff --git a/naoqi/modules/R2module/include/task.h b/naoqi/modules/R2module/include/task.h index 79be1e3..9c92c5d 100644 --- a/naoqi/modules/R2module/include/task.h +++ b/naoqi/modules/R2module/include/task.h @@ -107,6 +107,7 @@ class Task : public TaskBase // The task kinematic chain Rmath::KinChain* theKinChain; + int base_end; // Toggle positioning/velocity task bool positioningActive; @@ -117,18 +118,43 @@ class Task : public TaskBase std::vector path; // Current progress in the path int path_currentStep; + // Current target velocity + Eigen::VectorXd targetVelocity; + public: // Constructor - Task(int m, int n, int _priority, ConfigReader theConfigReader, int _base, int _ee); + Task(int m, int n, int _priority, ConfigReader theConfigReader); + Task(int m, int n, int _priority, const Rmath::KinChain& _kc, int _base = 0); // Destructor ~Task(); +// inline Rmath::KinChain* kinChain(){ return theKinChain; } + // Retrieve the current end-effector pose Eigen::VectorXd getCurrentPose(const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity()) const; + // Retrieve the current target velocity + inline const Eigen::VectorXd& getTargetVelocity() const + { + return targetVelocity; + } + // Retrieve the current target pose + const Eigen::VectorXd getTargetPose() const; + // Retrieve the kinchain joints information + void getJointsIDs(std::map* jointsIDs) const + { + theKinChain->getJointsIDs(jointsIDs); + } + // Check if the task is done + inline bool done() const { + return (path_currentStep == path.size()-1); + } + // Set a desired pose in the task space void setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints = 1, const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity()); + void setDesiredPose(const Eigen::VectorXd& idp, const Eigen::VectorXd& dp, int n_controlPoints = 1, + const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity()); // Set a desired configuration in the joint space void setDesiredConfiguration(const Eigen::VectorXd& desiredConf, int n_controlPoints); // Reset the current target pose (either in joint or task space) @@ -143,7 +169,7 @@ class Task : public TaskBase const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity() ); // Update function - void update(const Eigen::VectorXd& q, const Eigen::VectorXd& desiredVel, + void update(const Eigen::VectorXd& _q, const Eigen::VectorXd& desiredVel, double K = 1.0, const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity()); }; diff --git a/naoqi/modules/R2module/src/configReader.cpp b/naoqi/modules/R2module/src/configReader.cpp index c0ff5a2..1327135 100644 --- a/naoqi/modules/R2module/src/configReader.cpp +++ b/naoqi/modules/R2module/src/configReader.cpp @@ -10,46 +10,38 @@ #include "configReader.h" - -ConfigReader::ConfigReader(std::string dh_file_path, std::string params_file_path) +ConfigReader::ConfigReader(std::string params_file_path) { - dhReader(dh_file_path); - if(params_file_path != ""){ - paramsReader(params_file_path); - } + paramsReader(params_file_path); } -void ConfigReader::extractJointParams(std::vector* _min, std::vector* _max, std::vector* _zero_pose, int _i, int _j) -{ - _min->clear(); - _max->clear(); - _zero_pose->clear(); + void ConfigReader::setKinChain(std::string dh_file_path) + { + transformations.clear(); + kinChainJointIDs.clear(); + dhReader(dh_file_path); + } - for(int i=_i; i<_j; ++i) - { - _min->push_back( minBounds.at(i) ); - _max->push_back( maxBounds.at(i) ); - _zero_pose->push_back( zero_pose.at(i) ); - } +std::vector ConfigReader::getJointParams(std::string _id) +{ + assert( jointParams.find(_id) != jointParams.end() ); + return jointParams[_id]; } soth::VectorBound ConfigReader::extractJointBounds() { - soth::VectorBound bounds(5);//minBounds.size()); -// for(int i=0; irow(i) << minBounds.at(i), maxBounds.at(i); - } + for(int index=0; indexrow(index) << jointParams[jointID.at(index)].at(2), + jointParams[jointID.at(index)].at(1); } void ConfigReader::storeJointsID(std::vector* _jointID) @@ -57,6 +49,11 @@ void ConfigReader::storeJointsID(std::vector* _jointID) *_jointID = jointID; } +void ConfigReader::kinChainJointsID(std::vector* _kcJointID) +{ + *_kcJointID = kinChainJointIDs; +} + void ConfigReader::dhReader(std::string dh_file_path) { std::ifstream cfg_dh; @@ -67,6 +64,8 @@ void ConfigReader::dhReader(std::string dh_file_path) return; } + kinChainJointIDs.clear(); + std::string line; while(cfg_dh.good()) { @@ -78,6 +77,7 @@ void ConfigReader::dhReader(std::string dh_file_path) std::back_inserter >(tokens)); if(tokens.empty()) continue; + if(tokens.at(0) == "#") continue; // Canonical DH transform case if (tokens.at(0) == "H") @@ -86,6 +86,7 @@ void ConfigReader::dhReader(std::string dh_file_path) valueReader(tokens.at(2)), to_double(tokens.at(3)), 0.0 ); + kinChainJointIDs.push_back(tokens.at(6)); transformations.push_back(dh_transform); } // Fixed translation case @@ -100,7 +101,7 @@ void ConfigReader::dhReader(std::string dh_file_path) else if (tokens.at(0) == "R") { Rmath::Transform* rot = new Rmath::Rotation( to_double(tokens.at(1)), - to_double(tokens.at(2)) , + to_double(tokens.at(2)), to_double(tokens.at(3)), valueReader(tokens.at(4)) ); transformations.push_back(rot); @@ -108,12 +109,12 @@ void ConfigReader::dhReader(std::string dh_file_path) if (tokens.at(0) == "}") break; } + cfg_dh.close(); } void ConfigReader::paramsReader(std::string params_file_path) { - std::fstream cfg_params; cfg_params.open(params_file_path.c_str(), std::fstream::in); if(cfg_params == NULL) @@ -134,12 +135,19 @@ void ConfigReader::paramsReader(std::string params_file_path) if(tokens.empty()) continue; if( tokens.at(0) == "[" ) continue; - if( tokens.at(0) == "{" || tokens.at(0) == "}" ) continue; + if( tokens.at(0) == "#" ) continue; + if( tokens.at(0) == "{" ) continue; + + if( tokens.at(0) == "}" ) break; + + std::vector params; + params.push_back(valueReader(tokens.at(0))); + params.push_back(to_double(tokens.at(2))); + params.push_back(to_double(tokens.at(1))); + + jointParams.insert(std::pair >(tokens.at(3), params) ); - zero_pose.push_back( valueReader(tokens.at(0)) ); - minBounds.push_back( to_double(tokens.at(1)) ); - maxBounds.push_back( to_double(tokens.at(2)) ); - jointID.push_back( tokens.at(3) ); + jointID.push_back(tokens.at(3)); } cfg_params.close(); } diff --git a/naoqi/modules/R2module/src/libmath/kinChain.cpp b/naoqi/modules/R2module/src/libmath/kinChain.cpp index 45f9062..c51f673 100644 --- a/naoqi/modules/R2module/src/libmath/kinChain.cpp +++ b/naoqi/modules/R2module/src/libmath/kinChain.cpp @@ -13,79 +13,46 @@ #define INFO(x) std::cout << "[R2 module::KinChain] " << x << std::endl; - -/* - * Class constructor using a ConfigReader class object. - * The chain of transformation is extracted from the .cfg file using indinces _i, _j and then - * the private containers are initialized. - */ -Rmath::KinChain::KinChain(ConfigReader& theConfigReader, int _i, int _j) +Rmath::KinChain::KinChain( ConfigReader& theConfigReader ) { // Extract the chain of transformations transformations = theConfigReader.Transformations(); // Extract from the general .cfg file all the information about joints - std::vector jointBounds_max, jointBounds_min, jointZeroPose; - theConfigReader.extractJointParams(&jointBounds_min, &jointBounds_max, &jointZeroPose, _i, _j); - // Check for dimensions consistency - assert(jointBounds_max.size() == jointBounds_min.size()); - assert(jointBounds_max.size() <= transformations.size()); - assert(jointZeroPose.size() >= 0); + std::vector kcJointIds; + theConfigReader.kinChainJointsID(&kcJointIds); - // Detect the actual joints across the kinematic chain and fill the joints container int joint_count = 0; for(unsigned int i = 0; i < transformations.size(); ++i) { - // A joints is identified by a DH-transform - if ( transformations.at(i)->getType() == Rmath::dh_transform ) + if( transformations.at(i)->getType() == Rmath::dh_transform ) { - // Look for data about the current joint - if(jointBounds_min.size() != 0 && jointBounds_max.size() != 0 && jointZeroPose.size() != 0) - { - // Set the zero-value of the joint (if present) - if(jointZeroPose.at(joint_count) != 0.0) - transformations.at(i)->update(jointZeroPose.at(joint_count)); - // Generate a Joint class object and push it in the container - Rmath::KinChain::Joint current_q(jointZeroPose.at(joint_count), - jointBounds_max.at(joint_count), - jointBounds_min.at(joint_count)); - joints[i] = current_q; - } - // Create a joint with no mechanical bounds and initial value = 0.0 otherwise - else - { - Rmath::KinChain::Joint current_q (0.0); - joints[i] = current_q; - } + std::vector jointParams; + jointParams = theConfigReader.getJointParams( kcJointIds.at(joint_count) ); + + Rmath::KinChain::Joint current_q( kcJointIds.at(joint_count), + joint_count, + jointParams.at(0), + jointParams.at(1), + jointParams.at(2) ); + + joints.insert( std::pair (i, current_q) ); + ++joint_count; } } + } /* * Class constructor overload using a vector of Rmath::Transform. */ -Rmath::KinChain::KinChain(std::vector _transf): transformations(_transf) -{ - // Detect the actual joints across the kinematic chain and fill the joints container - int joint_count = 0; - for(unsigned int i = 0; i < transformations.size(); ++i) - { - // A joints is identified by a DH-transform - if ( transformations.at(i)->getType() == Rmath::dh_transform ) - { - // Create a joint with no mechanical bounds and initial value = 0.0 - Rmath::KinChain::Joint current_q; - joints[i] = current_q; - - ++joint_count; - } - } - -} +Rmath::KinChain::KinChain(std::vector _transf, std::map _joints): + transformations(_transf), joints(_joints) +{} /* - * Class copy constructor (NOTE: shallow copy!). + * Class copy constructor (NOTE: deep copy!). */ Rmath::KinChain::KinChain(const Rmath::KinChain& kc) { @@ -121,6 +88,18 @@ const soth::VectorBound Rmath::KinChain::jointLimits() return bounds; } +/* TOCOMMENT */ +void Rmath::KinChain::getJointsIDs(std::map* jointsIDs) const +{ + assert(jointsIDs != NULL); + std::map::const_iterator i = joints.begin(); + while(i !=joints.end()) + { + jointsIDs->insert( std::pair( (i->second).id, (i->second).col ) ); + ++i; + } +} + /* * Get the current joint configuration (or any subset of it). */ @@ -182,7 +161,7 @@ void Rmath::KinChain::update(int first, int last, Eigen::VectorXd& q) if ((j >= first) && (j <= last) ) { // Update using the Transform base class method - joint_updater->second = q(i); + (joint_updater->second).value = q(i); transformations.at(joint_updater->first)->update(q(i)); ++i; } @@ -201,14 +180,13 @@ void Rmath::KinChain::update(const Eigen::VectorXd &q) while (joint_updater != joints.end()) { // Update using the Transform base class method - joint_updater->second = q(i); + (joint_updater->second).value = q(i); transformations.at(joint_updater->first)->update(q(i)); ++joint_updater; ++i; } } - /* * Compute the forward kinematics of the chain (or any subchain of it) as a homogeneous transformation matrix. */ @@ -286,17 +264,19 @@ Rmath::KinChain& Rmath::KinChain::operator=(const Rmath::KinChain& kc) std::map::const_iterator joint_iter = kc.joints.begin(); while (joint_iter != kc.joints.end()) { - Rmath::KinChain::Joint current_q((joint_iter->second).value, - (joint_iter->second).max, - (joint_iter->second).min); + Rmath::KinChain::Joint current_q( (joint_iter->second).id, + (joint_iter->second).col, + (joint_iter->second).value, + (joint_iter->second).max, + (joint_iter->second).min ); - joints[joint_iter->first] = current_q; + joints.insert( std::pair (joint_iter->first, current_q) ); ++joint_iter; } - // Re-fill the vector of Transform* using the input KinChain object (NOTE: shallow copy!) + // Re-fill the vector of Transform* using the input KinChain object (NOTE: deep copy!) for(int i=0; iclone() ); } /* @@ -308,11 +288,13 @@ Rmath::KinChain& Rmath::KinChain::operator+=(const Rmath::KinChain& kc) std::map::const_iterator joint_iter = kc.joints.begin(); while (joint_iter != kc.joints.end()) { - Rmath::KinChain::Joint current_q((joint_iter->second).value, - (joint_iter->second).max, - (joint_iter->second).min); + Rmath::KinChain::Joint current_q( (joint_iter->second).id, + (joint_iter->second).col, + (joint_iter->second).value, + (joint_iter->second).max, + (joint_iter->second).min); // Indices are shifted by the size of the precedent chain - joints[ transformations.size() + joint_iter->first] = current_q; + joints.insert( std::pair (transformations.size() + joint_iter->first, current_q) ); ++joint_iter; } @@ -328,6 +310,35 @@ Rmath::KinChain& Rmath::KinChain::operator+=(const Rmath::KinChain& kc) */ Rmath::KinChain Rmath::KinChain::operator+(const Rmath::KinChain& kc) { + // Add the new set of joints to the container + std::map _joints; + std::map::const_iterator joint_iter = joints.begin(); + while (joint_iter != joints.end()) + { + Rmath::KinChain::Joint current_q( (joint_iter->second).id, + (joint_iter->second).col, + (joint_iter->second).value, + (joint_iter->second).max, + (joint_iter->second).min); + + // Indices are shifted by the size of the precedent chain + _joints.insert( std::pair (joint_iter->first, current_q) ); + ++joint_iter; + } + std::map::const_iterator kcJoint_iter = kc.joints.begin(); + while (kcJoint_iter != kc.joints.end()) + { + Rmath::KinChain::Joint current_q( (kcJoint_iter->second).id, + joints.size() + (kcJoint_iter->second).col, + (kcJoint_iter->second).value, + (kcJoint_iter->second).max, + (kcJoint_iter->second).min); + + // Indices are shifted by the size of the precedent chain + _joints.insert( std::pair (transformations.size() + kcJoint_iter->first, current_q) ); + ++kcJoint_iter; + } + // First insert copies of the Transform objects of *this std::vector transf; for(int i=0; i< transformations.size(); ++i) @@ -336,8 +347,9 @@ Rmath::KinChain Rmath::KinChain::operator+(const Rmath::KinChain& kc) for(int i=0; i clone() ); + // NOTE: any information on the joint bounds is lost (TO FIX. - return KinChain(transf); + return KinChain(transf, _joints); } /* @@ -349,4 +361,4 @@ std::ostream& Rmath::operator<<(std::ostream& out, const Rmath::KinChain& kc) kc.transformations.at(i)->print(out); return out; } - +// NOTE: any information on the joint bounds is lost (TO FIX. diff --git a/naoqi/modules/R2module/src/r2Module.cpp b/naoqi/modules/R2module/src/r2Module.cpp index bf24d7c..739d8da 100644 --- a/naoqi/modules/R2module/src/r2Module.cpp +++ b/naoqi/modules/R2module/src/r2Module.cpp @@ -17,9 +17,6 @@ */ #define INFO(x) std::cerr << "\033[22;34;1m" << "[r2module] " << x << "\033[0m" << std::endl; -//#define DEBUG_MODE -//#define TEST_KINCHAIN - namespace AL { @@ -90,19 +87,49 @@ R2Module::~R2Module() */ void R2Module::init() { - // Initialize the base kinematic chain (from Nao's left foot to body center) - ConfigReader theConfigReader( CONFIG_PATH + LEFT_LEG_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); - theConfigReader.storeJointsID(&jointID); - theKinChainLeftLeg = new Rmath::KinChain( theConfigReader, LLEG_CHAIN_BEGIN, LLEG_CHAIN_END ); - -#ifdef TEST_KINCHAIN - // Initialize the test kinematic chain (left arm) - ConfigReader theConfigReader_TMP( CONFIG_PATH + LEFT_ARM_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); - theKinChain_TMP = new Rmath::KinChain( theConfigReader_TMP, LARM_CHAIN_BEGIN, LARM_CHAIN_SIZE); -#endif + ConfigReader theConfigReader(CONFIG_PATH + JOINT_BOUNDS_CFG); + theConfigReader.storeJointsID(&jointID); + + // Initialize the base kinematic chain (from Nao's left foot to body center) + theConfigReader.setKinChain(CONFIG_PATH + LEFT_LEG_BASE_CFG); + theKinChain_LLBase = new Rmath::KinChain( theConfigReader ); + + // Initialize the base kinematic chain (from Nao's right foot to body center) + theConfigReader.setKinChain(CONFIG_PATH + RIGHT_LEG_BASE_CFG); + theKinChain_RLBase = new Rmath::KinChain( theConfigReader ); + + // Initialize the fixed transformation base-ankle + base_ankle << 0.0, 0.0, 1.0, BASE_ANKLE_X, + 0.0, -1.0, 0.0, BASE_ANKLE_Y, + 1.0, 0.0, 0.0, BASE_ANKLE_Z, + 0.0, 0.0, 0.0, 1.0; + + // initialize kinChain transofrmation w.r.t. CoM + theConfigReader.setKinChain(CONFIG_PATH + COM_HEAD_CFG); + CoM_Head = new Rmath::KinChain( theConfigReader ); + theConfigReader.setKinChain(CONFIG_PATH + COM_LEFT_ARM_CFG); + CoM_LeftArm = new Rmath::KinChain( theConfigReader ); + theConfigReader.setKinChain(CONFIG_PATH + COM_RIGHT_ARM_CFG); + CoM_RightArm = new Rmath::KinChain( theConfigReader ); + theConfigReader.setKinChain(CONFIG_PATH + COM_RIGHT_LEG_CFG); + CoM_RightLeg= new Rmath::KinChain( theConfigReader ); + theConfigReader.setKinChain(CONFIG_PATH + COM_LEFT_LEG_CFG); + CoM_LeftLeg = new Rmath::KinChain( theConfigReader ); + + // Initialize kinChains + theConfigReader.setKinChain(CONFIG_PATH + LEFT_LEG_CFG); + theKinChainLeftLeg = new Rmath::KinChain( theConfigReader ); + theConfigReader.setKinChain(CONFIG_PATH + RIGHT_LEG_CFG); + theKinChainRightLeg = new Rmath::KinChain( theConfigReader ); + theConfigReader.setKinChain(CONFIG_PATH + LEFT_ARM_CFG); + theKinChainLeftArm = new Rmath::KinChain( theConfigReader ); + theConfigReader.setKinChain(CONFIG_PATH + RIGHT_ARM_CFG); + theKinChainRightArm = new Rmath::KinChain( theConfigReader ); + theConfigReader.setKinChain(CONFIG_PATH + HEAD_CFG); + theKinChainHead = new Rmath::KinChain( theConfigReader ); #ifdef DEBUG_MODE - INFO("Initializing: building up base kinematic chain (left foot to body center)... "); + INFO("Initializing: building up kinematic chains... "); #endif /* ---------------------------------------- Proxies and threads initialization -------------------------------------------- */ @@ -169,42 +196,42 @@ void R2Module::init() INFO("Initializing: retrieving joint limits... "); #endif - /* ------------------------------ Other tasks: head (priority = 2), left arm (priority = 1) ------------------------------ */ + /* ------------------------------------------------- Other tasks -------------------------------------------------------- */ - // Extract joint bounds from the configuration file - ConfigReader headConfig( CONFIG_PATH + HEAD_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); - // Task initialization - Task* head = new Task(HEAD_TASK_DIM, HEAD_CHAIN_SIZE, HEAD_TASK_PRIORITY, headConfig, - HEAD_CHAIN_BEGIN, HEAD_CHAIN_END); - // Extract joint bounds from the configuration file - ConfigReader LpointingConfig( CONFIG_PATH + LEFT_ARM_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); // Task initialization - Task* Lpointing = new Task(LARM_TASK_DIM, LARM_CHAIN_SIZE, LARM_TASK_PRIORITY, LpointingConfig, - LARM_CHAIN_BEGIN, LARM_CHAIN_END); + Task* Rsupporting = new Task(RLEG_TASK_DIM, RLEG_CHAIN_SIZE, RLEG_TASK_PRIORITY, *theKinChainRightLeg); - // Extract joint bounds from the configuration file - ConfigReader RpointingConfig( CONFIG_PATH + RIGHT_ARM_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); - // Task initialization - Task* Rpointing = new Task(RARM_TASK_DIM, RARM_CHAIN_SIZE, RARM_TASK_PRIORITY, RpointingConfig, - RARM_CHAIN_BEGIN, RARM_CHAIN_END); + Task* Lsupporting = new Task(LLEG_TASK_DIM, LLEG_CHAIN_SIZE, LLEG_TASK_PRIORITY, *theKinChainLeftLeg); - // Push every task into the task map - taskMap.insert( std::pair ("Head task", head) ); - taskMap.insert( std::pair ("Left arm task", Lpointing) ); + Task* Lpointing = new Task(LARM_TASK_DIM, LARM_CHAIN_SIZE+LLEG_CHAIN_SIZE, + LARM_TASK_PRIORITY, *theKinChain_LLBase + *CoM_LeftArm + *theKinChainLeftArm, + LLEG_CHAIN_SIZE); + + Task* Rpointing = new Task(RARM_TASK_DIM, RARM_CHAIN_SIZE+LLEG_CHAIN_SIZE, + RARM_TASK_PRIORITY, *theKinChain_LLBase + *CoM_RightArm + *theKinChainRightArm, + LLEG_CHAIN_SIZE); + + Task* looking = new Task(HEAD_TASK_DIM, HEAD_CHAIN_SIZE+LLEG_CHAIN_SIZE, + HEAD_TASK_PRIORITY, *theKinChain_LLBase + *CoM_Head + *theKinChainHead, + LLEG_CHAIN_SIZE); + + // // Push every task into the task map + taskMap.insert( std::pair ("Head task", looking) ); taskMap.insert( std::pair ("Right arm task", Rpointing) ); + taskMap.insert( std::pair ("Left arm task", Lpointing) ); + taskMap.insert( std::pair ("Right leg task", Rsupporting) ); + taskMap.insert( std::pair ("Left leg task", Lsupporting) ); - // Storing tasks location with respect to the whole Nao kinematic chain - taskLoc.insert( std::pair (head, HEAD_CHAIN_BEGIN) ); - taskLoc.insert( std::pair (Lpointing, LARM_CHAIN_BEGIN) ); - taskLoc.insert( std::pair (Rpointing, RARM_CHAIN_BEGIN) ); #ifdef DEBUG_MODE INFO("Initializing tasks: "); INFO("- Joint limits, with priority " << jointLimits->getPriority()); - INFO("- Head task, with priority " << head->getPriority()); - INFO("- Left arm task, with priority " << Lpointing->getPriority()); + INFO("- Head task, with priority " << looking->getPriority()); INFO("- Right arm task, with priority " << Rpointing->getPriority()); + INFO("- Left arm task, with priority " << Lpointing->getPriority()); + INFO("- Right leg task, with priority " << Rsupporting->getPriority()); + INFO("- Left leg task, with priority " << Lsupporting->getPriority()); #endif } @@ -327,17 +354,30 @@ void R2Module::motion() INFO("Executing main loop..."); #endif - // Shift CoM-right arm - Eigen::Vector3d CoM_RA; - CoM_RA << COM_RARM_SHIFT_X, COM_RARM_SHIFT_Y, COM_RARM_SHIFT_Z; - // Shift CoM-left arm - Eigen::Vector3d CoM_LA; - CoM_LA << COM_LARM_SHIFT_X, COM_LARM_SHIFT_Y, COM_LARM_SHIFT_Z; +// // Turn tasks to active + jointLimits->activate(); + taskMap["Head task"]->activate(); + taskMap["Right arm task"]->activate(); + taskMap["Left arm task"]->activate(); + taskMap["Right leg task"]->activate(); + taskMap["Left leg task"]->activate(); // Defining the desired pose vectors in the task space - Eigen::VectorXd desiredHeadPose(HEAD_TASK_DIM), desiredLHandPose(LARM_TASK_DIM), desiredRHandPose(RARM_TASK_DIM); - Eigen::VectorXd desiredHeadPosition(3), desiredRHandPosition(3), desiredLHandPosition(3); - Eigen::VectorXd desiredHeadOrientation(3), desiredRHandOrientation(3), desiredLHandOrientation(3); + Eigen::VectorXd desiredHeadPose(HEAD_TASK_DIM), + desiredLHandPose(LARM_TASK_DIM), + desiredRHandPose(RARM_TASK_DIM), + desiredRLegPose(RLEG_TASK_DIM), + desiredLLegPose(LLEG_TASK_DIM); + Eigen::VectorXd desiredHeadPosition(3), + desiredRHandPosition(3), + desiredLHandPosition(3), + desiredRLegPosition(3), + desiredLLegPosition(3); + Eigen::VectorXd desiredHeadOrientation(3), + desiredRHandOrientation(3), + desiredLHandOrientation(3), + desiredRLegOrientation(3), + desiredLLegOrientation(3); // head position in 3D space desiredHeadPosition << HEAD_DESIRED_X, HEAD_DESIRED_Y, HEAD_DESIRED_Z; @@ -351,6 +391,14 @@ void R2Module::motion() desiredLHandPosition << LARM_DESIRED_X, LARM_DESIRED_Y, LARM_DESIRED_Z; // left arm orientation in 3D space desiredLHandOrientation << LARM_DESIRED_ROLL, LARM_DESIRED_PITCH, LARM_DESIRED_YAW; + // Right leg position in 3D space + desiredRLegPosition << RLEG_DESIRED_X, RLEG_DESIRED_Y, RLEG_DESIRED_Z; + // Right leg orientation in 3D space + desiredRLegOrientation << RLEG_DESIRED_ROLL, RLEG_DESIRED_PITCH, RLEG_DESIRED_YAW; + // Left leg position in 3D space + desiredLLegPosition << LLEG_DESIRED_X, LLEG_DESIRED_Y, LLEG_DESIRED_Z; + // Left leg orientation in 3D space + desiredLLegOrientation << LLEG_DESIRED_ROLL, LLEG_DESIRED_PITCH, LLEG_DESIRED_YAW; if( HEAD_TASK_DIM > 3 ) desiredHeadPose << desiredHeadPosition, desiredHeadOrientation.head(HEAD_TASK_DIM-3); @@ -367,43 +415,90 @@ void R2Module::motion() else desiredRHandPose << desiredRHandPosition.head(RARM_TASK_DIM); -// taskMap["Head task"]->setDesiredConfiguration(desiredHeadPose.head(HEAD_TASK_DIM), HEAD_TASK_NSTEPS); - taskMap["Head task"]->setDesiredPose(desiredHeadPose.head(HEAD_TASK_DIM), HEAD_TASK_NSTEPS, - Rmath::hTranslation(Eigen::Vector3d::Zero())); + if( RLEG_TASK_DIM > 3 ) + desiredRLegPose << desiredRLegPosition, desiredRLegOrientation.head(RLEG_TASK_DIM-3); + else + desiredRLegPose << desiredRLegPosition.head(RLEG_TASK_DIM); + + if( LLEG_TASK_DIM > 3 ) + desiredLLegPose << desiredLLegPosition, desiredLLegOrientation.head(LLEG_TASK_DIM-3); + else + desiredLLegPose << desiredLLegPosition.head(LLEG_TASK_DIM); + + Eigen::Vector3d iDesiredPose; + iDesiredPose << 0.0, 48, 590; + + Eigen::Matrix4d h_CoMRL; + CoM_RightLeg->forward(&h_CoMRL); + if(taskMap["Right leg task"]->isActive()) + taskMap["Right leg task"]->setDesiredPose( desiredRLegPose.head(RLEG_TASK_DIM), RLEG_TASK_NSTEPS, h_CoMRL ); + + Eigen::Matrix4d h_CoMLL; + CoM_LeftLeg->forward(&h_CoMLL); + if(taskMap["Left leg task"]->isActive()) + taskMap["Left leg task"]->setDesiredPose( desiredLLegPose.head(LLEG_TASK_DIM), LLEG_TASK_NSTEPS, h_CoMLL ); + + if(taskMap["Head task"]->isActive()) + taskMap["Head task"]->setDesiredPose( desiredHeadPose.head(HEAD_TASK_DIM), HEAD_TASK_NSTEPS, base_ankle ); + + if(taskMap["Left arm task"]->isActive()) +#ifndef LARM_CIRCLE_TASK + taskMap["Left arm task"]->setDesiredPose(iDesiredPose, desiredLHandPose.head(LARM_TASK_DIM), LARM_TASK_NSTEPS, base_ankle ); +#else + taskMap["Left arm task"]->circularPathGenerator(desiredLHandPose.head(LARM_TASK_DIM), CIRCLE_Z_DEPTH, + LARM_TASK_NSTEPS, CIRCLE_RADIUS, CIRCLE_LAPS, base_ankle ); +#endif - taskMap["Left arm task"]->setDesiredPose(desiredLHandPose.head(LARM_TASK_DIM), LARM_TASK_NSTEPS, - Rmath::homX(-M_PI/2, CoM_LA) ); -// taskMap["Left arm task"]->circularPathGenerator(desiredLHandPose.head(LARM_TASK_DIM), -70, LARM_TASK_NSTEPS, 100, 3, -// Rmath::homX(-M_PI/2, CoM_LA)); +#ifndef RARM_LARM_JOINT_TASK + if(taskMap["Right arm task"]->isActive()) +#ifndef RARM_CIRCLE_TASK + taskMap["Right arm task"]->setDesiredPose(desiredRHandPose.head(RARM_TASK_DIM), RARM_TASK_NSTEPS, base_ankle ); +#else + taskMap["Right arm task"]->circularPathGenerator(desiredRHandPose.head(RARM_TASK_DIM), CIRCLE_Z_DEPTH, + RARM_TASK_NSTEPS, CIRCLE_RADIUS, CIRCLE_LAPS, base_ankle ); +#endif +#endif -// taskMap["Right arm task"]->setDesiredPose(desiredRHandPose.head(RARM_TASK_DIM), RARM_TASK_NSTEPS, -// Rmath::homX(-M_PI/2, CoM_RA) ); - taskMap["Right arm task"]->circularPathGenerator(desiredRHandPose.head(RARM_TASK_DIM), -70, RARM_TASK_NSTEPS, 100, 3, - Rmath::homX(-M_PI/2, CoM_RA)); #ifdef DEBUG_MODE INFO("Desired head position in space: [\n" << desiredHeadPose << "]"); INFO("Desired left hand position in space: [\n" << desiredLHandPose << "]"); +#ifndef RARM_LARM_JOINT_TASK INFO("Desired right hand position in space: [\n" << desiredRHandPose << "]"); #endif + INFO("Desired right foot position in space: [\n" << desiredRLegPose << "]"); + INFO("Desired Hip position in space: [\n" << desiredLLegPose << "]"); +#endif - // Turn tasks to active - jointLimits->activate(); - taskMap["Head task"]->activate(); - taskMap["Left arm task"]->activate(); - taskMap["Right arm task"]->activate(); + updateConstraints(getConfiguration()); - closeHand("RHand"); - openHand("LHand"); // Main loop while(true) { +#ifdef UP_DOWN_TASK + if ( (taskMap["Right leg task"]->done()) && (taskMap["Left leg task"]->done()) ) + { + UP_DOWN *= -1; + + Eigen::Matrix4d h_CoMRL, h_CoMLL; + CoM_RightLeg->forward(&h_CoMRL); + CoM_LeftLeg->forward(&h_CoMLL); + + Eigen::VectorXd desiredRLegPose = taskMap["Right leg task"]->getTargetPose(); + Eigen::VectorXd desiredLLegPose = taskMap["Left leg task"]->getTargetPose(); + desiredRLegPose(2) += UP_DOWN; + desiredLLegPose(2) += UP_DOWN; + taskMap["Right leg task"]->setDesiredPose(desiredRLegPose, RLEG_TASK_NSTEPS, h_CoMRL ); + taskMap["Left leg task"]->setDesiredPose(desiredLLegPose, LLEG_TASK_NSTEPS, h_CoMLL ); + } +#endif + // Retrieve current robot configuration Eigen::VectorXd q(jointID.size()); q = getConfiguration(); #ifdef DEBUG_MODE - INFO("Current joint configuration:"); + INFO("\nCurrent joint configuration:"); for(unsigned int i=0; i::iterator updater = taskSet.begin(); while( updater != taskSet.end() ) { + // Retrieve current kinchain joints information + std::map jointsIndicesMap; + (*updater)->getJointsIDs(&jointsIndicesMap); // Current task constraint matrix (restricted to the joint involved) Eigen::MatrixXd currentA_task = (*updater)->constraintMatrix(); // Rewrite the task constraint matrix with the complete joint configuration Eigen::MatrixXd currentA = Eigen::MatrixXd::Zero( currentA_task.rows(), JOINTS_NUM ); // The task constraint matrix is the only non-zero block in A - currentA.block( 0, taskLoc[*updater], currentA_task.rows(), currentA_task.cols() ) = currentA_task; + for (unsigned int i = 0; i < jointID.size(); ++i) + { + if (jointsIndicesMap.find(jointID.at(i)) != jointsIndicesMap.end()) + currentA.col(i) = currentA_task.col( jointsIndicesMap[jointID.at(i)] ); + else + currentA.col(i) = Eigen::VectorXd::Zero(currentA_task.rows()); + } A.push_back(currentA); b.push_back((*updater)->vectorBounds()); @@ -476,19 +580,8 @@ void R2Module::motion() hsolver.showActiveSet(std::cerr); INFO("Solution: [\n" << qdot << "]"); #endif - -#ifdef TEST_KINCHAIN - qdot = Eigen::VectorXd::Zero(q.size()); - Eigen::MatrixXd J_TMP_pinv; - Rmath::pseudoInverse(J_TMP, &J_TMP_pinv); - - qdot.segment(LARM_CHAIN_BEGIN) = J_TMP_pinv * r; - INFO("Solution: [\n" << qdot << "]"); -#endif - // Update the robot configuration with the obtained solution updateConfiguration(q + qdot*TIME_STEP); - } } @@ -519,15 +612,6 @@ Eigen::VectorXd R2Module::getConfiguration() */ void R2Module::updateConstraints(const Eigen::VectorXd& q) { - // Updating the base kinematic chain (left foot to body center) - Eigen::Matrix4d H_base; - Eigen::MatrixXd J_base(6, LLEG_CHAIN_SIZE); - // Left leg joints are those on the bottom part of the IDs vector - theKinChainLeftLeg->update(q.tail(LLEG_CHAIN_SIZE)); - // Compute kinematics - theKinChainLeftLeg->forward(&H_base); - theKinChainLeftLeg->differential(&J_base); - /* ------------------------- Joint bounds update -------------------------*/ Eigen::MatrixXd velBounds(q.size(), 2); @@ -539,35 +623,15 @@ void R2Module::updateConstraints(const Eigen::VectorXd& q) INFO(std::endl << *jointLimits); #endif -#ifdef TEST_KINCHAIN - INFO("Updating test kinematic chain (body to left hand): "); - q_TMP << q.segment(LARM_CHAIN_BEGIN); - INFO("Joint configuration:"); - for(unsigned int i=0; iupdate(q_TMP); - theKinChain_TMP->forward(&H_TMP); - theKinChain_TMP->differential(&J_TMP, 3); - - INFO("Direct kinematics transform: "); - INFO(std::endl << H_TMP); - INFO("Jacobian (linear velocities only): "); - INFO(std::endl << J_TMP); -#endif - /* -------------------- Head and arms tasks update --------------------*/ - // Shift CoM-left arm - Eigen::Vector3d CoM_LA; - CoM_LA << 0.0, 98.0, 100.0; - // Shift CoM-right arm - Eigen::Vector3d CoM_RA; - CoM_RA << 0.0, -98.0, 100.0; // Head task update - ( taskMap["Head task"] )->update( q.head(HEAD_CHAIN_SIZE), Eigen::VectorXd::Zero(HEAD_TASK_DIM), K_HEAD, - Rmath::hTranslation(Eigen::Vector3d::Zero())); + Eigen::VectorXd q_H (LLEG_CHAIN_SIZE+HEAD_CHAIN_SIZE); + q_H << q.segment(LLEG_CHAIN_BEGIN), + q.segment(HEAD_CHAIN_BEGIN); + if(taskMap["Head task"]->isActive()) + ( taskMap["Head task"] )->update( q_H, Eigen::VectorXd::Zero(HEAD_TASK_DIM), K_HEAD, base_ankle ); #ifdef DEBUG_MODE INFO("Head task constraint equation: "); @@ -575,9 +639,11 @@ void R2Module::updateConstraints(const Eigen::VectorXd& q) #endif // Left arm task update - ( taskMap["Left arm task"] )->update( q.segment(LARM_CHAIN_BEGIN), Eigen::VectorXd::Zero(LARM_TASK_DIM), - K_LARM, Rmath::homX(-M_PI/2, CoM_LA) ); -// ( taskMap["Left arm task"] )->update( q.segment(LARM_CHAIN_BEGIN), Eigen::VectorXd::Zero(LARM_CHAIN_SIZE), K_LARM); + Eigen::VectorXd q_LA (LLEG_CHAIN_SIZE+LARM_CHAIN_SIZE); + q_LA << q.segment(LLEG_CHAIN_BEGIN), + q.segment(LARM_CHAIN_BEGIN); + if( taskMap["Left arm task"]->isActive() ) + ( taskMap["Left arm task"] )->update( q_LA, Eigen::VectorXd::Zero(LARM_TASK_DIM), K_LARM, base_ankle ); #ifdef DEBUG_MODE INFO("Left arm task constraint equation: "); @@ -585,22 +651,73 @@ void R2Module::updateConstraints(const Eigen::VectorXd& q) #endif // Right arm task update - ( taskMap["Right arm task"] )->update( q.segment(RARM_CHAIN_BEGIN), Eigen::VectorXd::Zero(RARM_TASK_DIM), - K_RARM, Rmath::homX(-M_PI/2, CoM_RA) ); -// ( taskMap["Right arm task"] )->update( q.segment(RARM_CHAIN_BEGIN), Eigen::VectorXd::Zero(RARM_CHAIN_SIZE), K_RARM); + Eigen::VectorXd q_RA (LLEG_CHAIN_SIZE+RARM_CHAIN_SIZE); + q_RA << q.segment(LLEG_CHAIN_BEGIN), + q.segment(RARM_CHAIN_BEGIN); +#ifdef RARM_LARM_JOINT_TASK + if ( ARMS_TASK == MIMIC_TASK ) + { + Eigen::VectorXd desiredRHandPose = ( taskMap["Left arm task"] )->getTargetPose(); + assert(RARM_TASK_DIM == LARM_TASK_DIM); + assert(LARM_TASK_DIM >= 2); + desiredRHandPose(1) = desiredRHandPose(1)-MINIMUM_HANDS_DISTANCE; + + if (LARM_TASK_DIM > 3) + for (int i=3; i < LARM_TASK_DIM; ++i) + desiredRHandPose(i) = -desiredRHandPose(i); + + ( taskMap["Right arm task"] )->setDesiredPose(desiredRHandPose, 1, base_ankle ); + ( taskMap["Right arm task"] )->update( q_RA, Eigen::VectorXd::Zero(RARM_TASK_DIM), K_RARM, base_ankle ); + } + else if ( ARMS_TASK == MIRROR_TASK ) + { + Eigen::VectorXd desiredRHandVel = ( taskMap["Left arm task"] )->getTargetVelocity(); + + assert(RARM_TASK_DIM == LARM_TASK_DIM); + assert(LARM_TASK_DIM >= 2); + desiredRHandVel(1) = -desiredRHandVel(1); + + if (LARM_TASK_DIM > 3) + for (int i=3; i < LARM_TASK_DIM; ++i) + desiredRHandVel(i) = -desiredRHandVel(i); + + ( taskMap["Right arm task"] )->update( q_RA, desiredRHandVel, K_RARM, base_ankle ); + } +#else + if( taskMap["Right arm task"]->isActive() ) + ( taskMap["Right arm task"] )->update( q_RA, Eigen::VectorXd::Zero(RARM_TASK_DIM), K_RARM, base_ankle ); +#endif #ifdef DEBUG_MODE INFO("Right arm task constraint equation: "); INFO(std::endl << *( taskMap["Right arm task"] ) ); #endif -#ifdef TEST_KINCHAIN - INFO("Desired left hand position in space: [\n" << desiredLHandPose << "]"); -// r = K_HEAD * ( desiredLHandPose - H_TMP.topRightCorner(3,1) ) + Eigen::Vector3d::UnitZ()*100; - r = ( Eigen::Vector3d::UnitX()*100 + - Eigen::Vector3d::UnitY()*100 + - Eigen::Vector3d::UnitZ()*100 ); - INFO("New target velocity:\n[" << r << "]"); + // Right leg task update + Eigen::Matrix4d h_CoMRL; + CoM_RightLeg->forward(&h_CoMRL); + if( taskMap["Right leg task"]->isActive() ) + ( taskMap["Right leg task"] )->update( q.segment(RLEG_CHAIN_BEGIN), + Eigen::VectorXd::Zero(RLEG_TASK_DIM), + K_RLEG, h_CoMRL); + +#ifdef DEBUG_MODE + INFO("Right leg task constraint equation: "); + INFO(std::endl << *( taskMap["Right leg task"] ) ); +#endif + + + // Left leg task update + Eigen::Matrix4d h_CoMLL; + CoM_LeftLeg->forward(&h_CoMLL); + if( taskMap["Left leg task"]->isActive() ) + ( taskMap["Left leg task"] )->update( q.segment(LLEG_CHAIN_BEGIN), + Eigen::VectorXd::Zero(LLEG_TASK_DIM), + K_LLEG, h_CoMLL ); + +#ifdef DEBUG_MODE + INFO("Left leg task constraint equation: "); + INFO(std::endl << *( taskMap["Left leg task"] ) ); #endif } diff --git a/naoqi/modules/R2module/src/r2Module_backUp.cpp b/naoqi/modules/R2module/src/r2Module_backUp.cpp new file mode 100644 index 0000000..1ab93d6 --- /dev/null +++ b/naoqi/modules/R2module/src/r2Module_backUp.cpp @@ -0,0 +1,834 @@ +#include +#include + +#include +#include +#include +#include + +#include "r2Module.h" +#include "configReader.h" +#include "configParams.h" + +/** + TODO: + - force soth convergence (stop criteria) + - task formalization +*/ + +#define INFO(x) std::cerr << "\033[22;34;1m" << "[r2module] " << x << "\033[0m" << std::endl; +//#define DEBUG_MODE +//#define TEST_KINCHAIN + + +namespace AL +{ + +/* + * Module constructor using Naoqi API methods. + */ +R2Module::R2Module( boost::shared_ptr pBroker, const std::string& pName ): + ALModule(pBroker , pName), fRegisteredToVideoDevice(false) +{ + // Module description + setModuleDescription("Robotics2 Project"); + + // Bound methods definition and description + functionName( "registerToVideoDevice", getName(), "Register to the V.I.M." ); + BIND_METHOD( R2Module::registerToVideoDevice ); + + functionName( "unRegisterFromVideoDevice", getName(), "Unregister from the V.I.M." ); + BIND_METHOD( R2Module::unRegisterFromVideoDevice ); + + functionName( "getCurrentFrame", getName(), "Save an image received from the camera." ); + BIND_METHOD( R2Module::getCurrentFrame ); + +#ifdef DEBUG_MODE + INFO("Module generated."); +#endif +} + +/* + * Module destructor: unsubscribes proxies, destroys threads and deallocates all memory used + */ +R2Module::~R2Module() +{ + try + { + // Unregister the video module + if(fCamProxy) fCamProxy->unsubscribe(fVideoClientName); + // Reset all proxies + fCamProxy.reset(); + fMotionProxy.reset(); + fPostureProxy.reset(); + } + catch(const AL::ALError& e) + { + qiLogError("vision.R2Module") << e.toString() << std::endl; + } + + // Destroy thread + pthread_cancel( motionThreadId ); + pthread_cancel( cameraThreadId ); + + // Deallocate memory + if (jointLimits) delete jointLimits; + // Cleaning up containers + std::map::iterator destroyer = taskMap.begin(); + while (destroyer != taskMap.end()) + { + if(destroyer->second) delete destroyer->second; + ++destroyer; + } + taskMap.clear(); + taskSet.clear(); +} + +/* + * Method automatically called when the module is created: + * Initializes threads, proxy interfaces along with the private members of the module itself. + */ +void R2Module::init() +{ + // Initialize the base kinematic chain (from Nao's left foot to body center) + ConfigReader theConfigReader( CONFIG_PATH + LEFT_LEG_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); + theConfigReader.storeJointsID(&jointID); + theKinChainLeftLeg = new Rmath::KinChain( theConfigReader, LLEG_CHAIN_BEGIN, LLEG_CHAIN_END ); + +#ifdef TEST_KINCHAIN + // Initialize the test kinematic chain (left arm) + ConfigReader theConfigReader_TMP( CONFIG_PATH + LEFT_ARM_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); + theKinChain_TMP = new Rmath::KinChain( theConfigReader_TMP, LARM_CHAIN_BEGIN, LARM_CHAIN_SIZE); +#endif + +#ifdef DEBUG_MODE + INFO("Initializing: building up base kinematic chain (left foot to body center)... "); +#endif + + /* ---------------------------------------- Proxies and threads initialization -------------------------------------------- */ + + // Create a proxy for ALVideoDevice, ALMotion and ALRobotPosture + try + { + fCamProxy = boost::shared_ptr(new ALVideoDeviceProxy(getParentBroker())); + fMotionProxy = boost::shared_ptr(new ALMotionProxy(getParentBroker())); + fPostureProxy = boost::shared_ptr(new ALRobotPostureProxy(getParentBroker())); + fBallTrackerProxy = boost::shared_ptr(new ALRedBallTrackerProxy(getParentBroker())); + } + catch (const AL::ALError& e) + { + qiLogError("vision.R2Module") << "Error while getting proxy on ALVideoDevice. Error msg: " << e.toString() << std::endl; + R2Module::exit(); + return; + } + + // Check whether the proxy has been correctly initialized + if(!fCamProxy || !fMotionProxy || !fPostureProxy) + { + qiLogError("vision.AmrModule") << "Error while getting proxy on ALVideoDevice. Check ALVideoDevice is running." << std::endl; + R2Module::exit(); + return; + } + + // Initialize resolution, image size and color space + pResolution = AL::kQVGA; + pColorSpace = AL::kYuvColorSpace; + int imgWidth = 0, imgHeight = 0; + setSizeFromResolution(pResolution, imgWidth, imgHeight); + // Subscrive to the camera + registerToVideoDevice(); + if(fRegisteredToVideoDevice) + { + // Initialize image + fcurrImageHeader = cv::Mat::zeros(imgHeight, imgWidth, CV_8UC1); + + // Create the camera thread + pthread_create( &cameraThreadId, NULL, (void*(*)(void*)) visionTh, this); + // Create the motion thread + pthread_create( &motionThreadId, NULL, (void*(*)(void*)) motionTh, this); + +#ifdef DEBUG_MODE + INFO("Proxies and threads correctly initialized..."); +#endif + + /* -------------------------- First and foremost task (top priority): joint limits -------------------------------------- */ + + + // Extract joint bounds from the configuration file + jointBounds = Eigen::MatrixXd::Zero(JOINTS_NUM, 2); + theConfigReader.extractJointBounds(&jointBounds); + + // Convert joint ranges into velocity bound estimates + Eigen::MatrixXd velBounds(JOINTS_NUM, 2); + posBound2velBound(jointBounds, getConfiguration(), &velBounds); + + // Actual task initialization + jointLimits = new TaskBase(0, Eigen::MatrixXd::Identity( JOINTS_NUM, JOINTS_NUM ), velBounds); + +#ifdef DEBUG_MODE + INFO("Initializing: retrieving joint limits... "); +#endif + + /* ------------------------------ Other tasks: head (priority = 2), left arm (priority = 1) ------------------------------ */ + + // Extract joint bounds from the configuration file + ConfigReader headConfig( CONFIG_PATH + HEAD_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); + // Task initialization + Task* head = new Task(HEAD_TASK_DIM, HEAD_CHAIN_SIZE, HEAD_TASK_PRIORITY, headConfig, + HEAD_CHAIN_BEGIN, HEAD_CHAIN_END); + + // Extract joint bounds from the configuration file + ConfigReader LpointingConfig( CONFIG_PATH + LEFT_ARM_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); + // Task initialization + Task* Lpointing = new Task(LARM_TASK_DIM, LARM_CHAIN_SIZE, LARM_TASK_PRIORITY, LpointingConfig, + LARM_CHAIN_BEGIN, LARM_CHAIN_END); + + // Extract joint bounds from the configuration file + ConfigReader RpointingConfig( CONFIG_PATH + RIGHT_ARM_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); + // Task initialization + Task* Rpointing = new Task(RARM_TASK_DIM, RARM_CHAIN_SIZE, RARM_TASK_PRIORITY, RpointingConfig, + RARM_CHAIN_BEGIN, RARM_CHAIN_END); + + // Extract joint bounds from the configuration file + ConfigReader RlegConfig( CONFIG_PATH + RIGHT_LEG_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); + // Task initialization + Task* Rsupporting = new Task(RLEG_TASK_DIM, RLEG_CHAIN_SIZE, RLEG_TASK_PRIORITY, RlegConfig, + RLEG_CHAIN_BEGIN, RLEG_CHAIN_END); + + // Extract joint bounds from the configuration file + ConfigReader LlegConfig( CONFIG_PATH + LEFT_LEG_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); + // Task initialization + Task* Lsupporting = new Task(LLEG_TASK_DIM, LLEG_CHAIN_SIZE, LLEG_TASK_PRIORITY, LlegConfig, + LLEG_CHAIN_BEGIN, LLEG_CHAIN_END); + + // Push every task into the task map + taskMap.insert( std::pair ("Head task", head) ); + taskMap.insert( std::pair ("Right arm task", Rpointing) ); + taskMap.insert( std::pair ("Left arm task", Lpointing) ); + taskMap.insert( std::pair ("Right leg task", Rsupporting) ); + taskMap.insert( std::pair ("Left leg task", Lsupporting) ); + + // Storing tasks location with respect to the whole Nao kinematic chain + taskLoc.insert( std::pair (head, HEAD_CHAIN_BEGIN) ); + taskLoc.insert( std::pair (Rpointing, RARM_CHAIN_BEGIN) ); + taskLoc.insert( std::pair (Lpointing, LARM_CHAIN_BEGIN) ); + taskLoc.insert( std::pair (Rsupporting, RLEG_CHAIN_BEGIN) ); + taskLoc.insert( std::pair (Lsupporting, LLEG_CHAIN_BEGIN) ); + +#ifdef DEBUG_MODE + INFO("Initializing tasks: "); + INFO("- Joint limits, with priority " << jointLimits->getPriority()); + INFO("- Head task, with priority " << head->getPriority()); + INFO("- Right arm task, with priority " << Rpointing->getPriority()); + INFO("- Left arm task, with priority " << Lpointing->getPriority()); + INFO("- Right leg task, with priority " << Rsupporting->getPriority()); + INFO("- Left leg task, with priority " << Lsupporting>getPriority()); +#endif + } + +} + +/* + * Method automatically called when the module is destroyed. + */ +void R2Module::exit() +{ + AL::ALModule::exit(); +} + +/* + * Register/unregister to the camera proxy. + */ +void R2Module::registerToVideoDevice() +{ + // Check if another module has subscribed already + if (fRegisteredToVideoDevice) + { + throw ALError(getName(), "registerToVideoDevice()", "A video module has already been " + "registered. Call unRegisterFromVideoDevice() before trying to register a new module."); + } + // Release any image header previously allocated. + if (!fcurrImageHeader.empty()) fcurrImageHeader.release(); + + if(fCamProxy) + fVideoClientName = fCamProxy->subscribeCamera("r2Module", 1, pResolution, pColorSpace, 30); + qiLogInfo("vision.r2Module") << "Module registered as " << fVideoClientName << std::endl; + + // Registration successful, set flag to true + fRegisteredToVideoDevice = true; +} + +void R2Module::unRegisterFromVideoDevice() +{ + // Check if the module is actually registered + if (!fRegisteredToVideoDevice) + { + throw ALError(getName(), "unRegisterFromVideoDevice()", "No video module is currently " + "registered! Call registerToVideoDevice first."); + } + // Release any image header previously allocated. + if (!fcurrImageHeader.empty()) fcurrImageHeader.release(); + + qiLogInfo("vision.R2Module") << "Unregister " << fVideoClientName << " module..." << std::endl; + if(fCamProxy) + fCamProxy->unsubscribe(fVideoClientName); + qiLogInfo("vision.R2Module") << "Done." << std::endl; + + // Unregistration successful, set flag to false. + fRegisteredToVideoDevice = false; +} + +/* + * Retrieve the current camera image. + */ +void R2Module::getCurrentFrame() +{ + // Check if a video module is actually registered + if (!fRegisteredToVideoDevice) + throw ALError(getName(), "saveImageRemote()", "No video module is currently " + "registered! Call registerToVideoDevice() first."); + + // Image request via the camera proxy + ALValue frame = fCamProxy->getImageRemote(fVideoClientName); + + // Check for correctness of the camera frame + if (frame.getType()!= ALValue::TypeArray && frame.getSize() != 7) + throw ALError(getName(), "getImages", "Invalid image returned."); + + // Put a time stamp on the image header + const long long timeStamp = ((long long)(int)frame[4])*1000000LL + ((long long)(int)frame[5]); + time = (int)(timeStamp/1000000LL); + // Retrieve image data + fcurrImageHeader.data = (uchar*) frame[6].GetBinary(); + + // Prompt the image in a window + if ((char) cv::waitKey(33) != 27) + cv::imshow("Frame", fcurrImageHeader); + + // Release the proxy + fCamProxy->releaseImage(fVideoClientName); +} + +/* + * The vision process. + */ +void R2Module::vision() +{ +// fBallTrackerProxy->startTracker(); + // Wait a while before starting acquisition + qi::os::sleep(1.5); + // Main loop + while(true) + { + qi::os::msleep(100); + /* WORK IN PROGRESS */ +// INFO("ball position w.r.t. CoM: \n"<stopTracker(); +} + +/* + * The motion process: + * - At each time step, the current robot configuration is retrieved to update all the robot tasks; + * - Active tasks are then pushed into a priority-ordered set, so to generate a task hierarchy for the HQP solver; + * - Once the solver is configured, active search is enabled and a feasible solution is sought; + * - The solution obtained for q_dot is integrated in discrete time and eventually used to update the robot configuration. + */ +void R2Module::motion() +{ + // Initialize the posture proxy + fPostureProxy->goToPosture("StandInit", 0.5f); + +#ifdef DEBUG_MODE + INFO("Posture proxy initialized."); + INFO("Turning tasks to 'active'... "); + INFO("Executing main loop..."); +#endif + + // Shift CoM-right arm + Eigen::Vector3d CoM_RA; + CoM_RA << COM_RARM_SHIFT_X, COM_RARM_SHIFT_Y, COM_RARM_SHIFT_Z; + // Shift CoM-left arm + Eigen::Vector3d CoM_LA; + CoM_LA << COM_LARM_SHIFT_X, COM_LARM_SHIFT_Y, COM_LARM_SHIFT_Z; + + // Shift CoM-right leg + Eigen::Vector3d CoM_RL; + CoM_RL << COM_RLEG_SHIFT_X, COM_RLEG_SHIFT_Y, COM_RLEG_SHIFT_Z; + // Shift CoM-left leg + Eigen::Vector3d Base_Lankle; + Base_Lankle << BASE_LANKLE_SHIFT_X, BASE_LANKLE_SHIFT_Y, BASE_LANKLE_SHIFT_Z; + + // Defining the desired pose vectors in the task space + Eigen::VectorXd desiredHeadPose(HEAD_TASK_DIM), desiredLHandPose(LARM_TASK_DIM), desiredRHandPose(RARM_TASK_DIM), + desiredRLegPose(RLEG_TASK_DIM), desiredLLegPose(LLEG_TASK_DIM); + Eigen::VectorXd desiredHeadPosition(3), desiredRHandPosition(3), desiredLHandPosition(3), + desiredRLegPosition(3), desiredLLegPosition(3); + Eigen::VectorXd desiredHeadOrientation(3), desiredRHandOrientation(3), desiredLHandOrientation(3), + desiredRLegOrientation(3), desiredLLegOrientation(3); + + // head position in 3D space + desiredHeadPosition << HEAD_DESIRED_X, HEAD_DESIRED_Y, HEAD_DESIRED_Z; + // head orientation in 3D space + desiredHeadOrientation << HEAD_DESIRED_ROLL, HEAD_DESIRED_PITCH, HEAD_DESIRED_YAW; + // Right arm position in 3D space + desiredRHandPosition << RARM_DESIRED_X, RARM_DESIRED_Y, RARM_DESIRED_Z; + // Right arm orientation in 3D space + desiredRHandOrientation << RARM_DESIRED_ROLL, RARM_DESIRED_PITCH, RARM_DESIRED_YAW; + // left arm position in 3D space + desiredLHandPosition << LARM_DESIRED_X, LARM_DESIRED_Y, LARM_DESIRED_Z; + // left arm orientation in 3D space + desiredLHandOrientation << LARM_DESIRED_ROLL, LARM_DESIRED_PITCH, LARM_DESIRED_YAW; + // Right leg position in 3D space + desiredRLegPosition << RLEG_DESIRED_X, RLEG_DESIRED_Y, RLEG_DESIRED_Z; + // Right leg orientation in 3D space + desiredRLegOrientation << RLEG_DESIRED_ROLL, RLEG_DESIRED_PITCH, RLEG_DESIRED_YAW; + // Left leg position in 3D space + desiredLLegPosition << LLEG_DESIRED_X, LLEG_DESIRED_Y, LLEG_DESIRED_Z; + // Left leg orientation in 3D space + desiredLLegOrientation << LLEG_DESIRED_ROLL, LLEG_DESIRED_PITCH, LLEG_DESIRED_YAW; + + if( HEAD_TASK_DIM > 3 ) + desiredHeadPose << desiredHeadPosition, desiredHeadOrientation.head(HEAD_TASK_DIM-3); + else + desiredHeadPose << desiredHeadPosition.head(HEAD_TASK_DIM); + + if( LARM_TASK_DIM > 3 ) + desiredLHandPose << desiredLHandPosition, desiredLHandOrientation.head(LARM_TASK_DIM-3); + else + desiredLHandPose << desiredLHandPosition.head(LARM_TASK_DIM); + + if( RARM_TASK_DIM > 3 ) + desiredRHandPose << desiredRHandPosition, desiredRHandOrientation.head(RARM_TASK_DIM-3); + else + desiredRHandPose << desiredRHandPosition.head(RARM_TASK_DIM); + + if( RLEG_TASK_DIM > 3 ) + desiredRLegPose << desiredRLegPosition, desiredRLegOrientation.head(RLEG_TASK_DIM-3); + else + desiredRLegPose << desiredRLegPosition.head(RLEG_TASK_DIM); + + if( LLEG_TASK_DIM > 3 ) + desiredLLegPose << desiredLLegPosition, desiredLLegOrientation.head(LLEG_TASK_DIM-3); + else + desiredLLegPose << desiredLLegPosition.head(LLEG_TASK_DIM); + + taskMap["Right leg task"]->setDesiredPose(desiredRLegPose.head(RLEG_TASK_DIM), RLEG_TASK_NSTEPS, + Rmath::homX(-M_PI/4, CoM_RL) ); + +// taskMap["Head task"]->setDesiredConfiguration(desiredHeadPose.head(HEAD_TASK_DIM), HEAD_TASK_NSTEPS); +// taskMap["Head task"]->setDesiredPose(desiredHeadPose.head(HEAD_TASK_DIM), HEAD_TASK_NSTEPS, +// Rmath::hTranslation(Eigen::Vector3d::Zero())); + +// taskMap["Left arm task"]->setDesiredPose(desiredLHandPose.head(LARM_TASK_DIM), LARM_TASK_NSTEPS, +// Rmath::homX(-M_PI/2, CoM_LA) ); +// taskMap["Left arm task"]->circularPathGenerator(desiredLHandPose.head(LARM_TASK_DIM), -70, LARM_TASK_NSTEPS, 100, 3, +// Rmath::homX(-M_PI/2, CoM_LA)); + +// taskMap["Right arm task"]->setDesiredPose(desiredRHandPose.head(RARM_TASK_DIM), RARM_TASK_NSTEPS, +// Rmath::homX(-M_PI/2, CoM_RA) ); +// taskMap["Right arm task"]->circularPathGenerator(desiredRHandPose.head(RARM_TASK_DIM), -70, RARM_TASK_NSTEPS, 100, 3, +// Rmath::homX(-M_PI/2, CoM_RA)); + +#ifdef DEBUG_MODE + INFO("Desired head position in space: [\n" << desiredHeadPose << "]"); + INFO("Desired left hand position in space: [\n" << desiredLHandPose << "]"); + INFO("Desired right hand position in space: [\n" << desiredRHandPose << "]"); +#endif + + // Turn tasks to active + jointLimits->activate(); +// taskMap["Head task"]->activate(); +// taskMap["Left arm task"]->activate(); +// taskMap["Right arm task"]->activate(); + taskMap["Right leg task"]->activate(); + + + closeHand("RHand"); + openHand("LHand"); + bool init = true; + // Main loop + while(true) + { +// if(init) +// { +// INFO("starting pointing task... "); +// taskMap["Left arm task"]->setDesiredPose(desiredLHandPose.head(LARM_TASK_DIM), +// LARM_TASK_NSTEPS, +// Rmath::homX(-M_PI/2, CoM_LA) ); +//// taskMap["Right arm task"]->circularPathGenerator(desiredRHandPose.head(RARM_TASK_DIM), -70, +//// RARM_TASK_NSTEPS, 100, 3, +//// Rmath::homX(-M_PI/2, CoM_RA)); +// init = false; +// } +// if( taskMap["Left arm task"]->isConverged() ) +// init = true; + +// if(init) +// { +// INFO("starting circular path... "); +// taskMap["Left arm task"]->circularPathGenerator(desiredLHandPose.head(LARM_TASK_DIM), -50, +// LARM_TASK_NSTEPS, 70, 3, +// Rmath::homX(-M_PI/2, CoM_LA)); +// init=false; +// } + + + // Retrieve current robot configuration + Eigen::VectorXd q(jointID.size()); + q = getConfiguration(); + +#ifdef DEBUG_MODE + INFO("Current joint configuration:"); + for(unsigned int i=0; i::const_iterator adder; + for ( adder = taskMap.begin(); adder != taskMap.end(); ++adder ) + if( (adder->second)->isActive()) + taskSet.insert(adder->second); + + // Initialize the HQP solver + soth::HCOD hsolver( q.size(), taskSet.size()+1 ); + Eigen::VectorXd qdot(q.size()); + +#ifdef DEBUG_MODE + INFO("Initializing HQP solver..."); + INFO( "Active tasks: " << (taskSet.size()+1) << " out of " << (taskMap.size()+1) ); + INFO("Generating stack..."); + + int task_i = 2; +#endif + // Set up the hierarchy + std::vector A; + std::vector b; + // Joint limits first (top priority task) + A.push_back(jointLimits->constraintMatrix()); + b.push_back(jointLimits->vectorBounds()); + // All the remaining tasks (in descending order of priority) + std::set::iterator updater = taskSet.begin(); + while( updater != taskSet.end() ) + { + // Current task constraint matrix (restricted to the joint involved) + Eigen::MatrixXd currentA_task = (*updater)->constraintMatrix(); + + // Rewrite the task constraint matrix with the complete joint configuration + Eigen::MatrixXd currentA = Eigen::MatrixXd::Zero( currentA_task.rows(), JOINTS_NUM ); + // The task constraint matrix is the only non-zero block in A + currentA.block( 0, taskLoc[*updater], currentA_task.rows(), currentA_task.cols() ) = currentA_task; + + A.push_back(currentA); + b.push_back((*updater)->vectorBounds()); + ++updater; + +#ifdef DEBUG_MODE + INFO("Task n. " << task_i << ", constraint matrix (enlarged):\n" << currentA); + ++task_i; +#endif + } + hsolver.pushBackStages(A, b); + + // Configure the solver + hsolver.setDamping(0.0); + hsolver.setInitialActiveSet(); + +#ifdef DEBUG_MODE + INFO("Configuring HQP solver..."); + INFO("Start active search... "); +#endif + // Run the solver + hsolver.activeSearch(qdot); + // Trim ssolution + Rmath::trim(&qdot); + +#ifdef DEBUG_MODE + INFO("Done."); + INFO("Active set: "); + hsolver.showActiveSet(std::cerr); + INFO("Solution: [\n" << qdot << "]"); +#endif + +#ifdef TEST_KINCHAIN + qdot = Eigen::VectorXd::Zero(q.size()); + Eigen::MatrixXd J_TMP_pinv; + Rmath::pseudoInverse(J_TMP, &J_TMP_pinv); + + qdot.segment(LARM_CHAIN_BEGIN) = J_TMP_pinv * r; + INFO("Solution: [\n" << qdot << "]"); +#endif + + // Update the robot configuration with the obtained solution + updateConfiguration(q + qdot*TIME_STEP); + + } +} + +/* + * Retrieve the current joint configuration with the motion proxy. + */ +Eigen::VectorXd R2Module::getConfiguration() +{ + // Store the resulting values in a Eigen::VectorXd + Eigen::VectorXd currentConfiguration(jointID.size()); + + AL::ALValue jointId, angles; + + // Use Naoqi ID for each joint to request the joint current measured value + for(int i=0; i getAngles( jointId, true ); + + for(int i=0; i update(q.tail(LLEG_CHAIN_SIZE)); + // Compute kinematics + theKinChainLeftLeg->forward(&H_base); + theKinChainLeftLeg->differential(&J_base); + + /*fast DEBUG*/ + INFO( std::endl << *taskMap["Right leg task"]->kinChain() ); + Eigen::Matrix4d H_RL; + (taskMap["Right leg task"]->kinChain())->forward(&H_RL); + INFO("H right Leg \n"<< H_RL); + + /* ------------------------- Joint bounds update -------------------------*/ + + Eigen::MatrixXd velBounds(q.size(), 2); + posBound2velBound(jointBounds, q, &velBounds); + jointLimits->setVectorBounds(velBounds); + +#ifdef DEBUG_MODE + INFO("Updating joint bounds: "); + INFO(std::endl << *jointLimits); +#endif + +#ifdef TEST_KINCHAIN + INFO("Updating test kinematic chain (body to left hand): "); + q_TMP << q.segment(LARM_CHAIN_BEGIN); + INFO("Joint configuration:"); + for(unsigned int i=0; iupdate(q_TMP); + theKinChain_TMP->forward(&H_TMP); + theKinChain_TMP->differential(&J_TMP, 3); + + INFO("Direct kinematics transform: "); + INFO(std::endl << H_TMP); + INFO("Jacobian (linear velocities only): "); + INFO(std::endl << J_TMP); +#endif + + /* -------------------- Head and arms tasks update --------------------*/ + + // Shift CoM-right arm + Eigen::Vector3d CoM_RA; + CoM_RA << COM_RARM_SHIFT_X, COM_RARM_SHIFT_Y, COM_RARM_SHIFT_Z; + // Shift CoM-left arm + Eigen::Vector3d CoM_LA; + CoM_LA << COM_LARM_SHIFT_X, COM_LARM_SHIFT_Y, COM_LARM_SHIFT_Z; + + // Shift CoM-right leg + Eigen::Vector3d CoM_RL; + CoM_RL << COM_RLEG_SHIFT_X, COM_RLEG_SHIFT_Y, COM_RLEG_SHIFT_Z; + // Shift CoM-left leg + Eigen::Vector3d Base_Lankle; + Base_Lankle << BASE_LANKLE_SHIFT_X, BASE_LANKLE_SHIFT_Y, BASE_LANKLE_SHIFT_Z; + + // Head task update + ( taskMap["Head task"] )->update( q.head(HEAD_CHAIN_SIZE), Eigen::VectorXd::Zero(HEAD_TASK_DIM), K_HEAD, + Rmath::hTranslation(Eigen::Vector3d::Zero())); + +#ifdef DEBUG_MODE + INFO("Head task constraint equation: "); + INFO(std::endl << *( taskMap["Head task"] ) ); +#endif + + // Left arm task update + ( taskMap["Left arm task"] )->update( q.segment(LARM_CHAIN_BEGIN), Eigen::VectorXd::Zero(LARM_TASK_DIM), + K_LARM, Rmath::homX(-M_PI/2, CoM_LA) ); +// ( taskMap["Left arm task"] )->update( q.segment(LARM_CHAIN_BEGIN), Eigen::VectorXd::Zero(LARM_CHAIN_SIZE), K_LARM); + +#ifdef DEBUG_MODE + INFO("Left arm task constraint equation: "); + INFO(std::endl << *( taskMap["Left arm task"] ) ); +#endif + + // Right arm task update + ( taskMap["Right arm task"] )->update( q.segment(RARM_CHAIN_BEGIN), Eigen::VectorXd::Zero(RARM_TASK_DIM), + K_RARM, Rmath::homX(-M_PI/2, CoM_RA) ); +// ( taskMap["Right arm task"] )->update( q.segment(RARM_CHAIN_BEGIN), Eigen::VectorXd::Zero(RARM_CHAIN_SIZE), K_RARM); + +#ifdef DEBUG_MODE + INFO("Right arm task constraint equation: "); + INFO(std::endl << *( taskMap["Right arm task"] ) ); +#endif + + // Right leg task update + ( taskMap["Right leg task"] )->update( q.segment(RLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(RLEG_TASK_DIM), + K_RLEG, Rmath::homX(-M_PI/4, CoM_RL) ); +// ( taskMap["Right leg task"] )->update( q.segment(RLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(RLEG_CHAIN_SIZE), K_RLEG); + +#ifdef DEBUG_MODE + INFO("Right arm task constraint equation: "); + INFO(std::endl << *( taskMap["Right leg task"] ) ); +#endif + +#ifdef TEST_KINCHAIN + INFO("Desired left hand position in space: [\n" << desiredLHandPose << "]"); +// r = K_HEAD * ( desiredLHandPose - H_TMP.topRightCorner(3,1) ) + Eigen::Vector3d::UnitZ()*100; + r = ( Eigen::Vector3d::UnitX()*100 + + Eigen::Vector3d::UnitY()*100 + + Eigen::Vector3d::UnitZ()*100 ); + INFO("New target velocity:\n[" << r << "]"); +#endif + +} + +/* + * Call Naoqi motion proxy to actually execute the motion. + */ +bool R2Module::updateConfiguration(const Eigen::VectorXd& delta_q) +{ + +#ifdef TEST_KINCHAIN + Eigen::VectorXd delta_q_ = delta_q; +#endif + + // Initialize variables + AL::ALValue jointId, angles, setStiff, zeroArray; + // Push into the vector all joint value increments to be requested + for(int i=0; i< delta_q.size(); ++i) + { + +#ifdef TEST_KINCHAIN + if (delta_q_(i) > 0) + { + if ( delta_q_(i) > jointBounds(i,1) ) + delta_q_(i) = jointBounds(i,1); + } + else + { + if ( delta_q_(i) < jointBounds(i,0) ) + delta_q_(i) = jointBounds(i,0); + } + + if(delta_q_(i) != 0) + { + jointId.arrayPush( jointID.at(i) ); + angles.arrayPush( delta_q_(i) ); + setStiff.arrayPush( 1.0 ); + zeroArray.arrayPush( 0.0 ); + } + continue; +#endif + if(delta_q(i) != 0) + { + jointId.arrayPush( jointID.at(i) ); +// angles.arrayPush( delta_q(i) ); + + if(jointID.at(i) == "RHipYawPitch") angles.arrayPush( /*delta_q(i)*/-M_PI/2 ); + else if(jointID.at(i) == "RHipRoll") angles.arrayPush( /*delta_q(i)*/-M_PI/4 ); + else if(jointID.at(i) == "RHipPitch") angles.arrayPush( /*delta_q(i)*/0.0 ); + else if(jointID.at(i) == "RKneePitch") angles.arrayPush( /*delta_q(i)*/0.0 ); + else if(jointID.at(i) == "RAnklePitch") angles.arrayPush( /*delta_q(i)*/0.0 ); + else if(jointID.at(i) == "RAnkleRoll") angles.arrayPush( /*delta_q(i)*/0.0 ); + else + angles.arrayPush( 0.0 ); + + setStiff.arrayPush( 1.0 ); + zeroArray.arrayPush( 0.0 ); + } + } + // Set up joint stiffness + fMotionProxy->stiffnessInterpolation( jointId, setStiff, 0.3f ); + fMotionProxy->setStiffnesses(jointId, setStiff); + + // Request motion to the motion proxy + fMotionProxy->setAngles(jointId, angles, 0.3f); + + // Reset stiffness + fMotionProxy->setStiffnesses(jointId, zeroArray); +} + +/* + * Estimate the joint velocity bounds given the actual joint limits and the current configuration. + */ +void R2Module::posBound2velBound( const Eigen::MatrixXd& posBound, const Eigen::VectorXd& configuration, Eigen::MatrixXd* velBound) +{ + Eigen::MatrixXd currConf(configuration.size(), 2); + currConf << configuration, configuration; + + *velBound = (posBound - currConf)/TIME_STEP; +} + +void R2Module::closeHand(const std::string& handID) +{ + if(handID == "both") + { + fMotionProxy->setStiffnesses("LHand", 1.0); + fMotionProxy->setStiffnesses("RHand", 1.0); + fMotionProxy->closeHand("LHand"); + fMotionProxy->closeHand("RHand"); + fMotionProxy->setStiffnesses("LHand", 0.0); + fMotionProxy->setStiffnesses("RHand", 0.0); + } + else + { + fMotionProxy->setStiffnesses(handID, 1.0); + fMotionProxy->closeHand(handID); + fMotionProxy->setStiffnesses(handID, 0.0); + } + +} + +void R2Module::openHand(const std::string& handID) +{ + if(handID == "both") + { + fMotionProxy->setStiffnesses("LHand", 1.0); + fMotionProxy->setStiffnesses("RHand", 1.0); + fMotionProxy->openHand("LHand"); + fMotionProxy->openHand("RHand"); + fMotionProxy->setStiffnesses("LHand", 0.0); + fMotionProxy->setStiffnesses("RHand", 0.0); + } + else + { + fMotionProxy->setStiffnesses(handID, 1.0); + fMotionProxy->openHand(handID); + fMotionProxy->setStiffnesses(handID, 0.0); + } + +} + +Eigen::Vector3d R2Module::getRedBallPosition() +{ + Eigen::Vector3d ballPosition; + ballPosition << fBallTrackerProxy->getPosition().at(0), + fBallTrackerProxy->getPosition().at(1), + fBallTrackerProxy->getPosition().at(2); + + return ballPosition; +} + +} // namespace AL diff --git a/naoqi/modules/R2module/src/task.cpp b/naoqi/modules/R2module/src/task.cpp index 1655162..3faa0cd 100644 --- a/naoqi/modules/R2module/src/task.cpp +++ b/naoqi/modules/R2module/src/task.cpp @@ -232,12 +232,31 @@ TaskBase TaskBase::operator--(int) * - A ConfigReader class object to initialize the task kinematic chain; * - Two indices _base, _ee identifying the task kinematic chain in the ConfigReader object file. */ -Task::Task(int m, int n, int _priority, ConfigReader theConfigReader, int _base, int _ee): +Task::Task(int m, int n, int _priority, ConfigReader theConfigReader): // Base class initalization TaskBase(_priority, Eigen::MatrixXd::Identity(m, n), Eigen::VectorXd::Zero(m), soth::Bound::BOUND_TWIN ), // Kinematic chain initialization - theKinChain( new Rmath::KinChain( theConfigReader, _base, _ee) ), - positioningActive(false), jointControlActive(false) + theKinChain( new Rmath::KinChain(theConfigReader) ), base_end(0), + // Flag initialization + positioningActive(false), jointControlActive(false), + // Target velocit initialization + targetVelocity(Eigen::VectorXd::Zero(m)) +{ +#ifdef DEBUG_MODE + INFO("Task kinematic chain:"); + INFO(theKinChain); +#endif +} + +Task::Task(int m, int n, int _priority, const Rmath::KinChain& _kc, int _base): + // Base class initalization + TaskBase(_priority, Eigen::MatrixXd::Identity(m, n), Eigen::VectorXd::Zero(m), soth::Bound::BOUND_TWIN ), + // Kinematic chain initialization + theKinChain( new Rmath::KinChain(_kc) ), base_end(_base), + // Flag initialization + positioningActive(false), jointControlActive(false), + // Target velocit initialization + targetVelocity(Eigen::VectorXd::Zero(m)) { #ifdef DEBUG_MODE INFO("Task kinematic chain:"); @@ -271,6 +290,15 @@ Eigen::VectorXd Task::getCurrentPose(const Eigen::Matrix4d& baseTransform) const return currentPose; } +/* TOCOMMENT */ +const Eigen::VectorXd Task::getTargetPose() const +{ + if(positioningActive) + return path.at(path_currentStep); + else + return getCurrentPose(); +} + void Task::setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints, const Eigen::Matrix4d& baseTransform) { // Re-computing direct kinematics @@ -290,6 +318,7 @@ void Task::setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints, const assert(dp.size() == initialPose.size()); + path.clear(); for (float i = 1.0; i <= n_controlPoints; ++i) path.push_back(initialPose + (i/static_cast(n_controlPoints)) * (dp - initialPose)); @@ -298,14 +327,48 @@ void Task::setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints, const if (jointControlActive) jointControlActive = false; } +void Task::setDesiredPose(const Eigen::VectorXd& idp, const Eigen::VectorXd& dp, int n_controlPoints, const Eigen::Matrix4d& baseTransform) +{ + // Re-computing direct kinematics + Eigen::Matrix4d H_chain; + theKinChain->forward((&H_chain)); + H_chain = baseTransform * H_chain; + + Eigen::VectorXd initialPose (constraint_matrix.rows()); + if(constraint_matrix.rows() > POSITION_TASK_DIM) + // Retrieving the position from the translation vector of the forward kinematics + // Retrieving the orientation from the Euler fixed frame x-y-z angles + initialPose << H_chain.topRightCorner(POSITION_TASK_DIM,1), + Rmath::xyzEulerAngles( H_chain.topLeftCorner(3,3) ).head(constraint_matrix.rows()-POSITION_TASK_DIM); + // If the task target is a position vector (A row size <= 3) just the translation vector of the forward kinematics is needed + else + initialPose << H_chain.col(POSITION_TASK_DIM).head(constraint_matrix.rows()); + + assert(idp.size() == initialPose.size()); + assert(dp.size() == initialPose.size()); + + path.clear(); + for (float i = 1.0; i <= n_controlPoints; ++i) + path.push_back(initialPose + (i/static_cast(n_controlPoints)) * (idp - initialPose)); + + for (float i = 1.0; i <= n_controlPoints; ++i) + path.push_back(idp + (i/static_cast(n_controlPoints)) * (dp - idp)); + + path_currentStep = 0; + positioningActive = true; + if (jointControlActive) jointControlActive = false; +} + void Task::setDesiredConfiguration(const Eigen::VectorXd& desiredConf, int n_controlPoints) { + Eigen::VectorXd initialConf = theKinChain->jointConfiguration(); assert(initialConf.size() == desiredConf.size()); constraint_matrix = Eigen::MatrixXd::Identity(initialConf.size(), initialConf.size()); bounds.setZero(initialConf.size(),1); + path.clear(); for (float i = 1.0; i <= n_controlPoints; ++i) path.push_back(initialConf + (i/static_cast(n_controlPoints)) * (desiredConf - initialConf)); @@ -317,28 +380,17 @@ void Task::setDesiredConfiguration(const Eigen::VectorXd& desiredConf, int n_con void Task::circularPathGenerator( const Eigen::VectorXd& dp, float z_shift, int n_controlPoints, float radius, int n, const Eigen::Matrix4d& baseTransform ) { + // Re-computing direct kinematics Eigen::Matrix4d H_chain; theKinChain->forward((&H_chain)); H_chain = baseTransform * H_chain; -// Eigen::Vector3d initialTranslation; -// initialTranslation << 218.7, 113, 112.31; -// Eigen::Vector3d transEEd = dp.head(3) - initialTranslation; -// INFO("desired vector: \n" << dp.head(3)); -// INFO("current vector: \n" << H_chain.topRightCorner(3,1)); -// /* very beginning Nao left hand translation -// 218.7, 113, 112.31 -// */ -// INFO("translation vector: \n" << transEEd); - + path.clear(); for (float i = 0.0; i < n*2*M_PI; i+= 2*M_PI/n_controlPoints) { Eigen::VectorXd ee_desiredPose_handframe(4); - ee_desiredPose_handframe << /*transEEd(0)+*/ radius*cos(i), - /*transEEd(1)+*/ radius*sin(i), - /*transEEd(2)+*/ z_shift, - 1.0; + ee_desiredPose_handframe << radius*cos(i), radius*sin(i), z_shift, 1.0; Rmath::trim(&ee_desiredPose_handframe); Eigen::VectorXd ee_desiredPose_CoMframe(dp.size()); @@ -361,11 +413,17 @@ void Task::circularPathGenerator( const Eigen::VectorXd& dp, float z_shift, int * Task update function: update the constraint matrix A=A(q) with a new value of q and replace the target vector * with a proportional/derivative control law of the type K*POSE_ERROR + DESIRED_VELOCITY. */ -void Task::update( const Eigen::VectorXd& q, const Eigen::VectorXd& desiredVel, double K, const Eigen::Matrix4d& baseTransform ) +void Task::update( const Eigen::VectorXd& _q, const Eigen::VectorXd& desiredVel, double K, const Eigen::Matrix4d& baseTransform ) { // Dimensions must be consistent assert(constraint_matrix.rows() == desiredVel.size()); - assert(constraint_matrix.cols() == q.size()); + assert(constraint_matrix.cols() == _q.size()); + + Eigen::VectorXd q(_q.size()); + q << -_q.head(base_end).reverse(), _q.tail(_q.size()-base_end); + + Eigen::Matrix4d H_chain; + theKinChain->forward((&H_chain)); // Cartesian space task if (!jointControlActive) @@ -421,21 +479,33 @@ void Task::update( const Eigen::VectorXd& q, const Eigen::VectorXd& desiredVel, else currentPose << H_chain.col(POSITION_TASK_DIM).head(constraint_matrix.rows()); - Eigen::VectorXd pose_error = path.at(path_currentStep) - currentPose; + Eigen::VectorXd pose_error; + if(path_currentStep < path.size()) + pose_error = path.at(path_currentStep) - currentPose; + else + pose_error = path.at(path.size()-1) - currentPose; // Updating the bound vector with the task error + a feedforward term for(int i=0; ijointConfiguration(); + Eigen::VectorXd joint_error; + if(path_currentStep < path.size()) + joint_error = path.at(path_currentStep) - theKinChain->jointConfiguration(); + else + joint_error = path.at(path.size()-1) - theKinChain->jointConfiguration(); // Updating the bound vector with the task error + a feedforward term for(int i=0; i Date: Tue, 17 Dec 2013 15:48:58 +0100 Subject: [PATCH 5/6] R2: r2module base module --- naoqi/modules/R2module/include/configParams.h | 59 ++- naoqi/modules/R2module/include/r2Module.h | 80 +++- naoqi/modules/R2module/include/task.h | 142 ++++-- naoqi/modules/R2module/include/taskManager.h | 75 ++++ naoqi/modules/R2module/src/r2Module.cpp | 409 +++--------------- naoqi/modules/R2module/src/task.cpp | 326 +++++++++----- naoqi/modules/R2module/src/taskManager.cpp | 391 +++++++++++++++++ 7 files changed, 955 insertions(+), 527 deletions(-) create mode 100644 naoqi/modules/R2module/include/taskManager.h create mode 100644 naoqi/modules/R2module/src/taskManager.cpp diff --git a/naoqi/modules/R2module/include/configParams.h b/naoqi/modules/R2module/include/configParams.h index 50afb37..390420a 100644 --- a/naoqi/modules/R2module/include/configParams.h +++ b/naoqi/modules/R2module/include/configParams.h @@ -3,7 +3,9 @@ #include -#define DEBUG_MODE +//#define DEBUG_MODE +//#define TASK_DEBUG +//#define TASK_MANAGER_DEBUG // Discrete integration time step @@ -30,8 +32,8 @@ static const double TIME_STEP = 1.0; // Config (.cfg) files directory and paths -//static const std::string CONFIG_PATH = "/home/claudio/Naoqi/NaoModules/r2module/config/"; static const std::string CONFIG_PATH = "/home/francesco/naoqi/modules/R2module/config/"; +//static const std::string CONFIG_PATH = "/home/claudio/Naoqi/NaoModules/r2module/config/"; // Joint parameters static const std::string JOINT_BOUNDS_CFG = "joints_params.cfg"; // Base kinematic chains @@ -92,7 +94,14 @@ static Eigen::Matrix4d base_ankle; /** ---------------------------- Task parameters ---------------------------- */ +static const float ACTIVATION_STEP = 1.00; +// Task names +static const std::string HEAD_TASK = "Head task"; +static const std::string RIGHT_ARM = "Right arm task"; +static const std::string LEFT_ARM = "Left arm task"; +static const std::string RIGHT_LEG = "Right leg task"; +static const std::string LEFT_LEG = "Left leg task"; // Task dimensions static const int HEAD_TASK_DIM = 6; @@ -104,7 +113,7 @@ static const int LLEG_TASK_DIM = 6; // Task priorities static const int HEAD_TASK_PRIORITY = 5; static const int RARM_TASK_PRIORITY = 3; -static const int LARM_TASK_PRIORITY = 2; +static const int LARM_TASK_PRIORITY = 3; static const int RLEG_TASK_PRIORITY = 1; static const int LLEG_TASK_PRIORITY = 1; @@ -117,25 +126,31 @@ static const double K_LLEG = 0.8; // Control points number static const int HEAD_TASK_NSTEPS = 1; -static const int RARM_TASK_NSTEPS = 30; -static const int LARM_TASK_NSTEPS = 30; +static const int RARM_TASK_NSTEPS = 15; +static const int LARM_TASK_NSTEPS = 8; static const int RLEG_TASK_NSTEPS = 30; static const int LLEG_TASK_NSTEPS = 30; +// Smooth transition step +static const double HEAD_TRANSITION_STEP = 1.0; +static const double RARM_TRANSITION_STEP = 1.0; +static const double LARM_TRANSITION_STEP = 1.0; +static const double RLEG_TRANSITION_STEP = 1.0; +static const double LLEG_TRANSITION_STEP = 1.0; -#define RARM_LARM_JOINT_TASK +//#define RARM_LARM_JOINT_TASK enum TaskType { MIRROR_TASK, MIMIC_TASK }; static const TaskType ARMS_TASK = MIRROR_TASK; -static const double MINIMUM_HANDS_DISTANCE = 350.0; +static const double MINIMUM_HANDS_DISTANCE = 100.0; // Circle trajectory task parameters //#define LARM_CIRCLE_TASK //#define RARM_CIRCLE_TASK static const double CIRCLE_Z_DEPTH = -70.0; // wrt ee-frame -static const double CIRCLE_RADIUS = 70.0; +static const double CIRCLE_RADIUS = 80.0; static const int CIRCLE_LAPS = 10; // Up-down with legs task @@ -146,7 +161,11 @@ static int UP_DOWN = 80.0; /** ------------------------------- Desired poses ------------------------------- */ - +static Eigen::VectorXd desiredHeadPose(6); +static Eigen::VectorXd desiredRHandPose(6); +static Eigen::VectorXd desiredLHandPose(6); +static Eigen::VectorXd desiredRLegPose(6); +static Eigen::VectorXd desiredLLegPose(6); //Head static const double HEAD_DESIRED_X = 0.0; // [mm] @@ -168,7 +187,7 @@ static const double LARM_DESIRED_Y = -HIP_PELVIS_SHIFT+TORSO_SHOULDER_Y+ELBOW_SH static const double LARM_DESIRED_Z = FOOT_HEIGHT+HIP_HEIGHT+HIP_TORSO+TORSO_SHOULDER_Z+WRIST_SHIFT_Z-9.9; static const double LARM_DESIRED_ROLL = 0.0; static const double LARM_DESIRED_PITCH = 0.0; -static const double LARM_DESIRED_YAW = M_PI_2; +static const double LARM_DESIRED_YAW = 0.0; // Right leg static const double RLEG_DESIRED_X = 20.0; static const double RLEG_DESIRED_Y = -HIP_PELVIS_SHIFT; @@ -193,4 +212,24 @@ static const double LLEG_DESIRED_PITCH = 0.0; static const double LLEG_DESIRED_YAW = 0.0; +static void initialization() +{ + // Initialize the fixed transformation base-ankle + base_ankle << 0.0, 0.0, 1.0, BASE_ANKLE_X, + 0.0, -1.0, 0.0, BASE_ANKLE_Y, + 1.0, 0.0, 0.0, BASE_ANKLE_Z, + 0.0, 0.0, 0.0, 1.0; + + desiredHeadPose << HEAD_DESIRED_X, HEAD_DESIRED_Y, HEAD_DESIRED_Z, + HEAD_DESIRED_ROLL, HEAD_DESIRED_PITCH, HEAD_DESIRED_YAW; + desiredRHandPose << RARM_DESIRED_X, RARM_DESIRED_Y, RARM_DESIRED_Z, + RARM_DESIRED_ROLL, RARM_DESIRED_PITCH, RARM_DESIRED_YAW; + desiredLHandPose << LARM_DESIRED_X, LARM_DESIRED_Y, LARM_DESIRED_Z, + LARM_DESIRED_ROLL, LARM_DESIRED_PITCH, LARM_DESIRED_YAW; + desiredRLegPose << RLEG_DESIRED_X, RLEG_DESIRED_Y, RLEG_DESIRED_Z, + RLEG_DESIRED_ROLL, RLEG_DESIRED_PITCH, RLEG_DESIRED_YAW; + desiredLLegPose << LLEG_DESIRED_X, LLEG_DESIRED_Y, LLEG_DESIRED_Z, + LLEG_DESIRED_ROLL, LLEG_DESIRED_PITCH, LLEG_DESIRED_YAW; +} + #endif diff --git a/naoqi/modules/R2module/include/r2Module.h b/naoqi/modules/R2module/include/r2Module.h index 1c89ae1..1b9d481 100644 --- a/naoqi/modules/R2module/include/r2Module.h +++ b/naoqi/modules/R2module/include/r2Module.h @@ -24,9 +24,6 @@ #include #include -// Soth includes -#include - // Libmath includes #include "libmath/kinChain.h" #include "libmath/Rutils.h" @@ -71,13 +68,76 @@ class R2Module : public AL::ALModule std::vector jointID; // Container for the jointBounds Eigen::MatrixXd jointBounds; - // Joint limits primary task - TaskBase* jointLimits; - // All initialized tasks - std::map taskMap; - // Stack of tasks currently active - std::set taskSet; + // Task handler and scheduler + class TaskManager + { + + private: + + // Pointer to the robot module + R2Module* module_ptr; + + // Joint limits primary task + TaskBase* jointLimits; + // All initialized tasks + std::map taskMap; + // Stack of tasks currently active + std::set taskSet; + // Set of tasks changing their priority value + std::map changingTaskMap; + + // Update a single task + void taskUpdate( const std::string& taskName, const Eigen::VectorXd& q ); + // Update all tasks fully enabled + void updateActiveTasks( const Eigen::VectorXd& q ); + // Update all tasks in a transition phase + void updateIntermediateTasks( const Eigen::VectorXd& q, const Eigen::VectorXd& partial_qdot ); + + // Compute a partial HQP solution involving only fully enabled tasks + void computePartialSolution(const Eigen::VectorXd& q, Eigen::VectorXd* partial_qdot); + // Compute the final HQP involving all enabled tasks + void computeCompleteSolution(const Eigen::VectorXd& q, const Eigen::VectorXd& partial_qdot, Eigen::VectorXd* qdot); + + public: + + // Constructor + TaskManager( R2Module* m_ptr ): module_ptr(m_ptr){} + // Destructor + ~TaskManager(); + + TaskBase* Rbound; + + // Set joint limits + void setJointLimits(const Eigen::MatrixXd& velBounds); + // Declare a new task + inline void createTask(const std::string& taskName, Task* taskPtr) + { + taskMap.insert( std::pair(taskName,taskPtr) ); + } + // Retrieve a reference to specific task + inline Task& task(const std::string& taskName) + { + assert(taskMap.find(taskName) != taskMap.end()); + return *(taskMap[taskName]); + } + // Change task priorities + inline void changePriority(const std::string& taskName, int new_priority) + { + // Legal priority value check + assert(new_priority > 0); + assert(taskMap.find(taskName) != taskMap.end()); + + if ( (taskMap[taskName]->getPriority() != new_priority) && + (changingTaskMap.find(taskName) == changingTaskMap.end()) ) + changingTaskMap.insert( std::pair(taskName, new_priority) ); + } + + // Solve the HQP problem with the current task set + void exec(const Eigen::VectorXd& q, Eigen::VectorXd* qdot); + }; + TaskManager taskManager; + // Common base kinematic chain (left foot to body center) Rmath::KinChain* theKinChain_LLBase; @@ -125,8 +185,6 @@ class R2Module : public AL::ALModule // Retrieve the current joint configuration Eigen::VectorXd getConfiguration(); - // Update every active task with the current joint configuration - void updateConstraints(const Eigen::VectorXd& q); // Call Naoqi motion proxy to actually execute the motion bool updateConfiguration(const Eigen::VectorXd &delta_q); diff --git a/naoqi/modules/R2module/include/task.h b/naoqi/modules/R2module/include/task.h index 9c92c5d..cbf1cac 100644 --- a/naoqi/modules/R2module/include/task.h +++ b/naoqi/modules/R2module/include/task.h @@ -2,7 +2,7 @@ * @class: Task * This class (and related hierarchy) defines a generic task in the HQP framework. * -* @file transform.h +* @file task.h * @author Claudio Delli Bovi, Chiara Picardi, Francesco Riccio */ @@ -14,6 +14,7 @@ #include "libmath/kinChain.h" #include "configReader.h" +#include "configParams.h" /** ------------------- TaskBase class declaration ------------------- */ /** @@ -26,16 +27,16 @@ class TaskBase { - protected: // Task description Eigen::MatrixXd constraint_matrix; soth::VectorBound bounds; + // Bound type + soth::Bound::bound_t boundType; + // Priority value int priority; - // Task status - bool active; public: @@ -53,11 +54,9 @@ class TaskBase // Task getters inline const soth::VectorBound& vectorBounds() const { return bounds; } inline const Eigen::MatrixXd& constraintMatrix() const { return constraint_matrix; } - inline bool isActive() const { return active; } - // Status togglers - inline void activate() { active = true; } - inline void stop() { active = false; } + inline const soth::Bound::bound_t getType() const { return boundType; } // Task setters + void setVectorBounds(const Eigen::VectorXd& b, soth::Bound::bound_t type); void setVectorBounds( const Eigen::MatrixXd& b ); void setVectorBounds( const soth::VectorBound& b ); void setConstraintMatrix( const Eigen::MatrixXd& A ); @@ -99,44 +98,111 @@ struct taskCmp * the desired pose and/or desired velocity. */ +enum status{ + active = 1, + inactive, + inactive2active, + active2inactive +}; class Task : public TaskBase { - private: // The task kinematic chain Rmath::KinChain* theKinChain; int base_end; - // Toggle positioning/velocity task - bool positioningActive; - // Toggle joint space/cartesian space control - bool jointControlActive; - - // Discrete path to desired pose - std::vector path; - // Current progress in the path - int path_currentStep; - // Current target velocity - Eigen::VectorXd targetVelocity; - + class Parameters + { + public: + + status taskStatus; + float activationValue; + float activationStep; + + // Partial solution of the HQP problem without the task + Eigen::VectorXd qd_n; + + // Toggle positioning/velocity task + bool positioningActive; + // Toggle joint space/cartesian space control + bool jointControlActive; + + // Discrete path to desired pose + std::vector path; + // Current progress in the path + int path_currentStep; + // Current target velocity + Eigen::VectorXd targetVelocity; + // Fixed base transform for the current task + Eigen::MatrixXd baseT; + + Parameters( status _s, float h, float ts, Eigen::VectorXd tv, Eigen::VectorXd _qd, Eigen::Matrix4d _baseT ): + taskStatus(_s), activationValue(h), activationStep(ts), qd_n(_qd), + positioningActive(false), jointControlActive(false), + path_currentStep(0), targetVelocity(tv), baseT(_baseT){} + + inline void increaseActivationValue() + { + activationValue += activationStep; + if(activationValue >= 1.0) + { + activationValue = 1.0; + taskStatus = active; + } + } + + inline void decreaseActivationValue() + { + activationValue -= activationStep; + if(activationValue <= 0.0) + { + activationValue = 0.0; + taskStatus = inactive; + } + } + + }; + + Parameters parameters; public: // Constructor - Task(int m, int n, int _priority, ConfigReader theConfigReader); - Task(int m, int n, int _priority, const Rmath::KinChain& _kc, int _base = 0); + Task(int m, int n, int _priority, ConfigReader theConfigReader, soth::Bound::bound_t boundType = soth::Bound::BOUND_TWIN ); + Task(int m, int n, int _priority, const Rmath::KinChain& _kc, int _base = 0, soth::Bound::bound_t boundType = soth::Bound::BOUND_TWIN ); // Destructor ~Task(); -// inline Rmath::KinChain* kinChain(){ return theKinChain; } + inline Rmath::KinChain* kinChain(){ return theKinChain; } + + inline status taskStatus(){ return parameters.taskStatus; } + void activate(float activationStep = ACTIVATION_STEP); + void stop( float decayStep = ACTIVATION_STEP); + + void set_qd(Eigen::VectorXd new_qd) + { + assert(new_qd.size() == parameters.qd_n.size()); + parameters.qd_n = new_qd; + } + void set_baseTransform(const Eigen::Matrix4d& _baseT) + { + parameters.baseT = _baseT; + } + +#ifdef TASK_MANAGER_DEBUG + inline float activationValue() const + { + return parameters.activationValue; + } +#endif // Retrieve the current end-effector pose - Eigen::VectorXd getCurrentPose(const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity()) const; + Eigen::VectorXd getCurrentPose() const; // Retrieve the current target velocity inline const Eigen::VectorXd& getTargetVelocity() const { - return targetVelocity; + return parameters.targetVelocity; } // Retrieve the current target pose const Eigen::VectorXd getTargetPose() const; @@ -146,31 +212,31 @@ class Task : public TaskBase theKinChain->getJointsIDs(jointsIDs); } // Check if the task is done - inline bool done() const { - return (path_currentStep == path.size()-1); + inline bool done() const + { + if ((parameters.taskStatus == inactive)) + return false; + else + return (parameters.path_currentStep == parameters.path.size()-1); } // Set a desired pose in the task space - void setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints = 1, - const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity()); - void setDesiredPose(const Eigen::VectorXd& idp, const Eigen::VectorXd& dp, int n_controlPoints = 1, - const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity()); + void setDesiredPose( const Eigen::VectorXd& dp, int n_controlPoints = 1 ); + void setDesiredPose( const Eigen::VectorXd& idp, const Eigen::VectorXd& dp, int n_controlPoints = 1 ); // Set a desired configuration in the joint space void setDesiredConfiguration(const Eigen::VectorXd& desiredConf, int n_controlPoints); // Reset the current target pose (either in joint or task space) inline void resetDesiredPose() { - positioningActive = false; - path.clear(); + parameters.positioningActive = false; + parameters.path.clear(); } // design a circular path - void circularPathGenerator( const Eigen::VectorXd& dp, float z_shift = 0.0, int n_controlPoints = 1, float radius = 0.0, int n = 1, - const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity() ); + void circularPathGenerator( const Eigen::VectorXd& dp, float z_shift = 0.0, int n_controlPoints = 1, float radius = 0.0, int n = 1 ); // Update function - void update(const Eigen::VectorXd& _q, const Eigen::VectorXd& desiredVel, - double K = 1.0, const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity()); + void update(const Eigen::VectorXd& _q, const Eigen::VectorXd& desiredVel, double K = 1.0 ); }; #endif diff --git a/naoqi/modules/R2module/include/taskManager.h b/naoqi/modules/R2module/include/taskManager.h new file mode 100644 index 0000000..81129cf --- /dev/null +++ b/naoqi/modules/R2module/include/taskManager.h @@ -0,0 +1,75 @@ +/** +* @class: TaskManager +* This class defines a scheduler for tasks in the HQP framework. +* +* @file taskManager.h +* @author Claudio Delli Bovi, Chiara Picardi, Francesco Riccio +*/ + +#ifndef TASK_MANAGER +#define TASK_MANAGER + +#include +#include +#include +#include "Eigen/Core" + +#include "task.h" +#include "configParams.h" + + +class R2Module; + +class TaskManager +{ + +private: + + // Pointer to the robot module + R2Module* module_ptr; + + // Joint limits primary task + TaskBase* jointLimits; + // All initialized tasks + std::map taskMap; + // Stack of tasks currently active + std::set taskSet; + + // Update a single task + void taskUpdate( const std::string& taskName, const Eigen::VectorXd& q ); + // Update all tasks fully enabled + void updateActiveTasks( const Eigen::VectorXd& q ); + // Update all tasks in a transition phase + void updateIntermediateTasks( const Eigen::VectorXd& q, const Eigen::VectorXd& partial_qdot ); + + // Compute a partial HQP solution involving only fully enabled tasks + void computePartialSolution(const Eigen::VectorXd& q, Eigen::VectorXd* partial_qdot); + // Compute the final HQP involving all enabled tasks + void computeCompleteSolution(const Eigen::VectorXd& q, const Eigen::VectorXd& partial_qdot, Eigen::VectorXd* qdot); + +public: + + // Constructor + TaskManager( R2Module* m_ptr ): module_ptr(m_ptr){} + // Destructor + ~TaskManager(); + + // Set joint limits + void setJointLimits(const Eigen::MatrixXd& velBounds); + // Declare a new task + inline void createTask(const std::string& taskName, Task* taskPtr) + { + taskMap.insert( std::pair(taskName,taskPtr) ); + } + // Retrieve a reference to specific task + inline Task& task(const std::string& taskName) + { + assert(taskMap.find(taskName) != taskMap.end()); + return *(taskMap[taskName]); + } + + // Solve the HQP problem with the current task set + void exec(const Eigen::VectorXd& q, Eigen::VectorXd* qdot); +}; + +#endif diff --git a/naoqi/modules/R2module/src/r2Module.cpp b/naoqi/modules/R2module/src/r2Module.cpp index 739d8da..70a6537 100644 --- a/naoqi/modules/R2module/src/r2Module.cpp +++ b/naoqi/modules/R2module/src/r2Module.cpp @@ -12,8 +12,8 @@ /** TODO: - - force soth convergence (stop criteria) - - task formalization + - rearraging priorities + - Simulations */ #define INFO(x) std::cerr << "\033[22;34;1m" << "[r2module] " << x << "\033[0m" << std::endl; @@ -25,7 +25,7 @@ namespace AL * Module constructor using Naoqi API methods. */ R2Module::R2Module( boost::shared_ptr pBroker, const std::string& pName ): - ALModule(pBroker , pName), fRegisteredToVideoDevice(false) + ALModule(pBroker , pName), fRegisteredToVideoDevice(false), taskManager(this) { // Module description setModuleDescription("Robotics2 Project"); @@ -67,18 +67,6 @@ R2Module::~R2Module() // Destroy thread pthread_cancel( motionThreadId ); pthread_cancel( cameraThreadId ); - - // Deallocate memory - if (jointLimits) delete jointLimits; - // Cleaning up containers - std::map::iterator destroyer = taskMap.begin(); - while (destroyer != taskMap.end()) - { - if(destroyer->second) delete destroyer->second; - ++destroyer; - } - taskMap.clear(); - taskSet.clear(); } /* @@ -87,6 +75,9 @@ R2Module::~R2Module() */ void R2Module::init() { + // Static initialization of parameters + initialization(); + ConfigReader theConfigReader(CONFIG_PATH + JOINT_BOUNDS_CFG); theConfigReader.storeJointsID(&jointID); @@ -98,12 +89,6 @@ void R2Module::init() theConfigReader.setKinChain(CONFIG_PATH + RIGHT_LEG_BASE_CFG); theKinChain_RLBase = new Rmath::KinChain( theConfigReader ); - // Initialize the fixed transformation base-ankle - base_ankle << 0.0, 0.0, 1.0, BASE_ANKLE_X, - 0.0, -1.0, 0.0, BASE_ANKLE_Y, - 1.0, 0.0, 0.0, BASE_ANKLE_Z, - 0.0, 0.0, 0.0, 1.0; - // initialize kinChain transofrmation w.r.t. CoM theConfigReader.setKinChain(CONFIG_PATH + COM_HEAD_CFG); CoM_Head = new Rmath::KinChain( theConfigReader ); @@ -190,7 +175,7 @@ void R2Module::init() posBound2velBound(jointBounds, getConfiguration(), &velBounds); // Actual task initialization - jointLimits = new TaskBase(0, Eigen::MatrixXd::Identity( JOINTS_NUM, JOINTS_NUM ), velBounds); + taskManager.setJointLimits(velBounds); #ifdef DEBUG_MODE INFO("Initializing: retrieving joint limits... "); @@ -217,16 +202,14 @@ void R2Module::init() LLEG_CHAIN_SIZE); // // Push every task into the task map - taskMap.insert( std::pair ("Head task", looking) ); - taskMap.insert( std::pair ("Right arm task", Rpointing) ); - taskMap.insert( std::pair ("Left arm task", Lpointing) ); - taskMap.insert( std::pair ("Right leg task", Rsupporting) ); - taskMap.insert( std::pair ("Left leg task", Lsupporting) ); - + taskManager.createTask(HEAD_TASK, looking); + taskManager.createTask(RIGHT_ARM, Rpointing); + taskManager.createTask(LEFT_ARM, Lpointing); + taskManager.createTask(RIGHT_LEG, Rsupporting); + taskManager.createTask(LEFT_LEG, Lsupporting); #ifdef DEBUG_MODE INFO("Initializing tasks: "); - INFO("- Joint limits, with priority " << jointLimits->getPriority()); INFO("- Head task, with priority " << looking->getPriority()); INFO("- Right arm task, with priority " << Rpointing->getPriority()); INFO("- Left arm task, with priority " << Lpointing->getPriority()); @@ -234,7 +217,6 @@ void R2Module::init() INFO("- Left leg task, with priority " << Lsupporting->getPriority()); #endif } - } /* @@ -314,7 +296,7 @@ void R2Module::getCurrentFrame() if ((char) cv::waitKey(33) != 27) cv::imshow("Frame", fcurrImageHeader); - // Release the proxy + // Release the proxystd::cout<<"jesus: \n"<releaseImage(fVideoClientName); } @@ -354,108 +336,48 @@ void R2Module::motion() INFO("Executing main loop..."); #endif -// // Turn tasks to active - jointLimits->activate(); - taskMap["Head task"]->activate(); - taskMap["Right arm task"]->activate(); - taskMap["Left arm task"]->activate(); - taskMap["Right leg task"]->activate(); - taskMap["Left leg task"]->activate(); - - // Defining the desired pose vectors in the task space - Eigen::VectorXd desiredHeadPose(HEAD_TASK_DIM), - desiredLHandPose(LARM_TASK_DIM), - desiredRHandPose(RARM_TASK_DIM), - desiredRLegPose(RLEG_TASK_DIM), - desiredLLegPose(LLEG_TASK_DIM); - Eigen::VectorXd desiredHeadPosition(3), - desiredRHandPosition(3), - desiredLHandPosition(3), - desiredRLegPosition(3), - desiredLLegPosition(3); - Eigen::VectorXd desiredHeadOrientation(3), - desiredRHandOrientation(3), - desiredLHandOrientation(3), - desiredRLegOrientation(3), - desiredLLegOrientation(3); - - // head position in 3D space - desiredHeadPosition << HEAD_DESIRED_X, HEAD_DESIRED_Y, HEAD_DESIRED_Z; - // head orientation in 3D space - desiredHeadOrientation << HEAD_DESIRED_ROLL, HEAD_DESIRED_PITCH, HEAD_DESIRED_YAW; - // Right arm position in 3D space - desiredRHandPosition << RARM_DESIRED_X, RARM_DESIRED_Y, RARM_DESIRED_Z; - // Right arm orientation in 3D space - desiredRHandOrientation << RARM_DESIRED_ROLL, RARM_DESIRED_PITCH, RARM_DESIRED_YAW; - // left arm position in 3D space - desiredLHandPosition << LARM_DESIRED_X, LARM_DESIRED_Y, LARM_DESIRED_Z; - // left arm orientation in 3D space - desiredLHandOrientation << LARM_DESIRED_ROLL, LARM_DESIRED_PITCH, LARM_DESIRED_YAW; - // Right leg position in 3D space - desiredRLegPosition << RLEG_DESIRED_X, RLEG_DESIRED_Y, RLEG_DESIRED_Z; - // Right leg orientation in 3D space - desiredRLegOrientation << RLEG_DESIRED_ROLL, RLEG_DESIRED_PITCH, RLEG_DESIRED_YAW; - // Left leg position in 3D space - desiredLLegPosition << LLEG_DESIRED_X, LLEG_DESIRED_Y, LLEG_DESIRED_Z; - // Left leg orientation in 3D space - desiredLLegOrientation << LLEG_DESIRED_ROLL, LLEG_DESIRED_PITCH, LLEG_DESIRED_YAW; - - if( HEAD_TASK_DIM > 3 ) - desiredHeadPose << desiredHeadPosition, desiredHeadOrientation.head(HEAD_TASK_DIM-3); - else - desiredHeadPose << desiredHeadPosition.head(HEAD_TASK_DIM); - - if( LARM_TASK_DIM > 3 ) - desiredLHandPose << desiredLHandPosition, desiredLHandOrientation.head(LARM_TASK_DIM-3); - else - desiredLHandPose << desiredLHandPosition.head(LARM_TASK_DIM); - - if( RARM_TASK_DIM > 3 ) - desiredRHandPose << desiredRHandPosition, desiredRHandOrientation.head(RARM_TASK_DIM-3); - else - desiredRHandPose << desiredRHandPosition.head(RARM_TASK_DIM); - - if( RLEG_TASK_DIM > 3 ) - desiredRLegPose << desiredRLegPosition, desiredRLegOrientation.head(RLEG_TASK_DIM-3); - else - desiredRLegPose << desiredRLegPosition.head(RLEG_TASK_DIM); + // Turn tasks to active + taskManager.task(HEAD_TASK).activate(HEAD_TRANSITION_STEP); + taskManager.task(RIGHT_ARM).activate(RARM_TRANSITION_STEP); + taskManager.task(LEFT_ARM).activate(LARM_TRANSITION_STEP); + taskManager.task(RIGHT_LEG).activate(RLEG_TRANSITION_STEP); + taskManager.task(LEFT_LEG).activate(LLEG_TRANSITION_STEP); - if( LLEG_TASK_DIM > 3 ) - desiredLLegPose << desiredLLegPosition, desiredLLegOrientation.head(LLEG_TASK_DIM-3); - else - desiredLLegPose << desiredLLegPosition.head(LLEG_TASK_DIM); + Eigen::Matrix4d h_CoMRL, h_CoMLL; + CoM_RightLeg->forward(&h_CoMRL); + CoM_LeftLeg->forward(&h_CoMLL); - Eigen::Vector3d iDesiredPose; - iDesiredPose << 0.0, 48, 590; + // Set fixed base transform for the tasks + taskManager.task(RIGHT_LEG).set_baseTransform(h_CoMRL); + taskManager.task(LEFT_LEG).set_baseTransform(h_CoMLL); + taskManager.task(HEAD_TASK).set_baseTransform(base_ankle); + taskManager.task(LEFT_ARM).set_baseTransform(base_ankle); + taskManager.task(RIGHT_ARM).set_baseTransform(base_ankle); - Eigen::Matrix4d h_CoMRL; - CoM_RightLeg->forward(&h_CoMRL); - if(taskMap["Right leg task"]->isActive()) - taskMap["Right leg task"]->setDesiredPose( desiredRLegPose.head(RLEG_TASK_DIM), RLEG_TASK_NSTEPS, h_CoMRL ); + if(taskManager.task(RIGHT_LEG).taskStatus() != inactive) + taskManager.task(RIGHT_LEG).setDesiredPose( desiredRLegPose.head(RLEG_TASK_DIM), RLEG_TASK_NSTEPS ); - Eigen::Matrix4d h_CoMLL; - CoM_LeftLeg->forward(&h_CoMLL); - if(taskMap["Left leg task"]->isActive()) - taskMap["Left leg task"]->setDesiredPose( desiredLLegPose.head(LLEG_TASK_DIM), LLEG_TASK_NSTEPS, h_CoMLL ); + if(taskManager.task(LEFT_LEG).taskStatus() != inactive) + taskManager.task(LEFT_LEG).setDesiredPose( desiredLLegPose.head(LLEG_TASK_DIM), LLEG_TASK_NSTEPS ); - if(taskMap["Head task"]->isActive()) - taskMap["Head task"]->setDesiredPose( desiredHeadPose.head(HEAD_TASK_DIM), HEAD_TASK_NSTEPS, base_ankle ); + if(taskManager.task(HEAD_TASK).taskStatus() != inactive) + taskManager.task(HEAD_TASK).setDesiredPose( desiredHeadPose.head(HEAD_TASK_DIM), HEAD_TASK_NSTEPS ); - if(taskMap["Left arm task"]->isActive()) + if(taskManager.task(LEFT_ARM).taskStatus() != inactive) #ifndef LARM_CIRCLE_TASK - taskMap["Left arm task"]->setDesiredPose(iDesiredPose, desiredLHandPose.head(LARM_TASK_DIM), LARM_TASK_NSTEPS, base_ankle ); + taskManager.task(LEFT_ARM).setDesiredPose( desiredLHandPose.head(LARM_TASK_DIM), LARM_TASK_NSTEPS ); #else - taskMap["Left arm task"]->circularPathGenerator(desiredLHandPose.head(LARM_TASK_DIM), CIRCLE_Z_DEPTH, - LARM_TASK_NSTEPS, CIRCLE_RADIUS, CIRCLE_LAPS, base_ankle ); + taskManager.task(LEFT_ARM).circularPathGenerator(desiredLHandPose.head(LARM_TASK_DIM), CIRCLE_Z_DEPTH, + LARM_TASK_NSTEPS, CIRCLE_RADIUS, CIRCLE_LAPS ); #endif #ifndef RARM_LARM_JOINT_TASK - if(taskMap["Right arm task"]->isActive()) + if(taskManager.task(RIGHT_ARM).taskStatus() != inactive) #ifndef RARM_CIRCLE_TASK - taskMap["Right arm task"]->setDesiredPose(desiredRHandPose.head(RARM_TASK_DIM), RARM_TASK_NSTEPS, base_ankle ); + taskManager.task(RIGHT_ARM).setDesiredPose(desiredRHandPose.head(RARM_TASK_DIM), RARM_TASK_NSTEPS ); #else - taskMap["Right arm task"]->circularPathGenerator(desiredRHandPose.head(RARM_TASK_DIM), CIRCLE_Z_DEPTH, - RARM_TASK_NSTEPS, CIRCLE_RADIUS, CIRCLE_LAPS, base_ankle ); + taskManager.task(RIGHT_ARM).circularPathGenerator(desiredRHandPose.head(RARM_TASK_DIM), CIRCLE_Z_DEPTH, + RARM_TASK_NSTEPS, CIRCLE_RADIUS, CIRCLE_LAPS ); #endif #endif @@ -470,116 +392,37 @@ void R2Module::motion() INFO("Desired Hip position in space: [\n" << desiredLLegPose << "]"); #endif - updateConstraints(getConfiguration()); - // Main loop while(true) { + #ifdef UP_DOWN_TASK - if ( (taskMap["Right leg task"]->done()) && (taskMap["Left leg task"]->done()) ) + if ( (taskManager.task(RIGHT_LEG).done()) && (taskManager.task(LEFT_LEG).done()) ) { UP_DOWN *= -1; - Eigen::Matrix4d h_CoMRL, h_CoMLL; - CoM_RightLeg->forward(&h_CoMRL); - CoM_LeftLeg->forward(&h_CoMLL); - - Eigen::VectorXd desiredRLegPose = taskMap["Right leg task"]->getTargetPose(); - Eigen::VectorXd desiredLLegPose = taskMap["Left leg task"]->getTargetPose(); + Eigen::VectorXd desiredRLegPose = taskManager.task(RIGHT_LEG).getTargetPose(); + Eigen::VectorXd desiredLLegPose = taskManager.task(LEFT_LEG).getTargetPose(); desiredRLegPose(2) += UP_DOWN; desiredLLegPose(2) += UP_DOWN; - taskMap["Right leg task"]->setDesiredPose(desiredRLegPose, RLEG_TASK_NSTEPS, h_CoMRL ); - taskMap["Left leg task"]->setDesiredPose(desiredLLegPose, LLEG_TASK_NSTEPS, h_CoMLL ); + taskManager.task(RIGHT_LEG).setDesiredPose(desiredRLegPose.head(RLEG_TASK_DIM), RLEG_TASK_NSTEPS); + taskManager.task(LEFT_LEG).setDesiredPose(desiredLLegPose.head(LLEG_TASK_DIM), LLEG_TASK_NSTEPS); } #endif - // Retrieve current robot configuration - Eigen::VectorXd q(jointID.size()); + Eigen::VectorXd q(JOINTS_NUM); q = getConfiguration(); -#ifdef DEBUG_MODE + #ifdef DEBUG_MODE INFO("\nCurrent joint configuration:"); - for(unsigned int i=0; i::const_iterator adder; - for ( adder = taskMap.begin(); adder != taskMap.end(); ++adder ) - if( (adder->second)->isActive()) - taskSet.insert(adder->second); + #endif - // Initialize the HQP solver - soth::HCOD hsolver( q.size(), taskSet.size()+1 ); - Eigen::VectorXd qdot(q.size()); + // Solve the HQP problem with the current task set + Eigen::VectorXd qdot(JOINTS_NUM); + taskManager.exec(q, &qdot); -#ifdef DEBUG_MODE - INFO("Initializing HQP solver..."); - INFO( "Active tasks: " << (taskSet.size()+1) << " out of " << (taskMap.size()+1) ); - INFO("Generating stack..."); - - int task_i = 2; -#endif - // Set up the hierarchy - std::vector A; - std::vector b; - // Joint limits first (top priority task) - A.push_back(jointLimits->constraintMatrix()); - b.push_back(jointLimits->vectorBounds()); - // All the remaining tasks (in descending order of priority) - std::set::iterator updater = taskSet.begin(); - while( updater != taskSet.end() ) - { - // Retrieve current kinchain joints information - std::map jointsIndicesMap; - (*updater)->getJointsIDs(&jointsIndicesMap); - // Current task constraint matrix (restricted to the joint involved) - Eigen::MatrixXd currentA_task = (*updater)->constraintMatrix(); - - // Rewrite the task constraint matrix with the complete joint configuration - Eigen::MatrixXd currentA = Eigen::MatrixXd::Zero( currentA_task.rows(), JOINTS_NUM ); - // The task constraint matrix is the only non-zero block in A - for (unsigned int i = 0; i < jointID.size(); ++i) - { - if (jointsIndicesMap.find(jointID.at(i)) != jointsIndicesMap.end()) - currentA.col(i) = currentA_task.col( jointsIndicesMap[jointID.at(i)] ); - else - currentA.col(i) = Eigen::VectorXd::Zero(currentA_task.rows()); - } - - A.push_back(currentA); - b.push_back((*updater)->vectorBounds()); - ++updater; - -#ifdef DEBUG_MODE - INFO("Task n. " << task_i << ", constraint matrix (enlarged):\n" << currentA); - ++task_i; -#endif - } - hsolver.pushBackStages(A, b); - - // Configure the solver - hsolver.setDamping(0.0); - hsolver.setInitialActiveSet(); - -#ifdef DEBUG_MODE - INFO("Configuring HQP solver..."); - INFO("Start active search... "); -#endif - // Run the solver - hsolver.activeSearch(qdot); - // Trim ssolution - Rmath::trim(&qdot); - -#ifdef DEBUG_MODE - INFO("Done."); - INFO("Active set: "); - hsolver.showActiveSet(std::cerr); - INFO("Solution: [\n" << qdot << "]"); -#endif // Update the robot configuration with the obtained solution updateConfiguration(q + qdot*TIME_STEP); } @@ -607,158 +450,16 @@ Eigen::VectorXd R2Module::getConfiguration() return currentConfiguration; } -/* - * Update every active task with the current joint configuration. - */ -void R2Module::updateConstraints(const Eigen::VectorXd& q) -{ - /* ------------------------- Joint bounds update -------------------------*/ - - Eigen::MatrixXd velBounds(q.size(), 2); - posBound2velBound(jointBounds, q, &velBounds); - jointLimits->setVectorBounds(velBounds); - -#ifdef DEBUG_MODE - INFO("Updating joint bounds: "); - INFO(std::endl << *jointLimits); -#endif - - /* -------------------- Head and arms tasks update --------------------*/ - - - // Head task update - Eigen::VectorXd q_H (LLEG_CHAIN_SIZE+HEAD_CHAIN_SIZE); - q_H << q.segment(LLEG_CHAIN_BEGIN), - q.segment(HEAD_CHAIN_BEGIN); - if(taskMap["Head task"]->isActive()) - ( taskMap["Head task"] )->update( q_H, Eigen::VectorXd::Zero(HEAD_TASK_DIM), K_HEAD, base_ankle ); - -#ifdef DEBUG_MODE - INFO("Head task constraint equation: "); - INFO(std::endl << *( taskMap["Head task"] ) ); -#endif - - // Left arm task update - Eigen::VectorXd q_LA (LLEG_CHAIN_SIZE+LARM_CHAIN_SIZE); - q_LA << q.segment(LLEG_CHAIN_BEGIN), - q.segment(LARM_CHAIN_BEGIN); - if( taskMap["Left arm task"]->isActive() ) - ( taskMap["Left arm task"] )->update( q_LA, Eigen::VectorXd::Zero(LARM_TASK_DIM), K_LARM, base_ankle ); - -#ifdef DEBUG_MODE - INFO("Left arm task constraint equation: "); - INFO(std::endl << *( taskMap["Left arm task"] ) ); -#endif - - // Right arm task update - Eigen::VectorXd q_RA (LLEG_CHAIN_SIZE+RARM_CHAIN_SIZE); - q_RA << q.segment(LLEG_CHAIN_BEGIN), - q.segment(RARM_CHAIN_BEGIN); -#ifdef RARM_LARM_JOINT_TASK - if ( ARMS_TASK == MIMIC_TASK ) - { - Eigen::VectorXd desiredRHandPose = ( taskMap["Left arm task"] )->getTargetPose(); - assert(RARM_TASK_DIM == LARM_TASK_DIM); - assert(LARM_TASK_DIM >= 2); - desiredRHandPose(1) = desiredRHandPose(1)-MINIMUM_HANDS_DISTANCE; - - if (LARM_TASK_DIM > 3) - for (int i=3; i < LARM_TASK_DIM; ++i) - desiredRHandPose(i) = -desiredRHandPose(i); - - ( taskMap["Right arm task"] )->setDesiredPose(desiredRHandPose, 1, base_ankle ); - ( taskMap["Right arm task"] )->update( q_RA, Eigen::VectorXd::Zero(RARM_TASK_DIM), K_RARM, base_ankle ); - } - else if ( ARMS_TASK == MIRROR_TASK ) - { - Eigen::VectorXd desiredRHandVel = ( taskMap["Left arm task"] )->getTargetVelocity(); - - assert(RARM_TASK_DIM == LARM_TASK_DIM); - assert(LARM_TASK_DIM >= 2); - desiredRHandVel(1) = -desiredRHandVel(1); - - if (LARM_TASK_DIM > 3) - for (int i=3; i < LARM_TASK_DIM; ++i) - desiredRHandVel(i) = -desiredRHandVel(i); - - ( taskMap["Right arm task"] )->update( q_RA, desiredRHandVel, K_RARM, base_ankle ); - } -#else - if( taskMap["Right arm task"]->isActive() ) - ( taskMap["Right arm task"] )->update( q_RA, Eigen::VectorXd::Zero(RARM_TASK_DIM), K_RARM, base_ankle ); -#endif - -#ifdef DEBUG_MODE - INFO("Right arm task constraint equation: "); - INFO(std::endl << *( taskMap["Right arm task"] ) ); -#endif - - // Right leg task update - Eigen::Matrix4d h_CoMRL; - CoM_RightLeg->forward(&h_CoMRL); - if( taskMap["Right leg task"]->isActive() ) - ( taskMap["Right leg task"] )->update( q.segment(RLEG_CHAIN_BEGIN), - Eigen::VectorXd::Zero(RLEG_TASK_DIM), - K_RLEG, h_CoMRL); - -#ifdef DEBUG_MODE - INFO("Right leg task constraint equation: "); - INFO(std::endl << *( taskMap["Right leg task"] ) ); -#endif - - - // Left leg task update - Eigen::Matrix4d h_CoMLL; - CoM_LeftLeg->forward(&h_CoMLL); - if( taskMap["Left leg task"]->isActive() ) - ( taskMap["Left leg task"] )->update( q.segment(LLEG_CHAIN_BEGIN), - Eigen::VectorXd::Zero(LLEG_TASK_DIM), - K_LLEG, h_CoMLL ); - -#ifdef DEBUG_MODE - INFO("Left leg task constraint equation: "); - INFO(std::endl << *( taskMap["Left leg task"] ) ); -#endif - -} - /* * Call Naoqi motion proxy to actually execute the motion. */ bool R2Module::updateConfiguration(const Eigen::VectorXd& delta_q) { - -#ifdef TEST_KINCHAIN - Eigen::VectorXd delta_q_ = delta_q; -#endif - // Initialize variables AL::ALValue jointId, angles, setStiff, zeroArray; // Push into the vector all joint value increments to be requested for(int i=0; i< delta_q.size(); ++i) { - -#ifdef TEST_KINCHAIN - if (delta_q_(i) > 0) - { - if ( delta_q_(i) > jointBounds(i,1) ) - delta_q_(i) = jointBounds(i,1); - } - else - { - if ( delta_q_(i) < jointBounds(i,0) ) - delta_q_(i) = jointBounds(i,0); - } - - if(delta_q_(i) != 0) - { - jointId.arrayPush( jointID.at(i) ); - angles.arrayPush( delta_q_(i) ); - setStiff.arrayPush( 1.0 ); - zeroArray.arrayPush( 0.0 ); - } - continue; -#endif if(delta_q(i) != 0) { jointId.arrayPush( jointID.at(i) ); diff --git a/naoqi/modules/R2module/src/task.cpp b/naoqi/modules/R2module/src/task.cpp index 3faa0cd..6889f44 100644 --- a/naoqi/modules/R2module/src/task.cpp +++ b/naoqi/modules/R2module/src/task.cpp @@ -9,7 +9,7 @@ #include "task.h" #define INFO(x) std::cerr << "\033[22;34;1m" << "[Task] " << x << "\033[0m" << std::endl; -//#define DEBUG_MODE +//#define TASK_DEBUG #define POSITION_TASK_DIM 3 @@ -24,7 +24,7 @@ * the class private members using their copy constructors. */ TaskBase::TaskBase(int _priority, const Eigen::MatrixXd& A, const soth::VectorBound& b): - priority(_priority), active(false) + boundType(b[0].getType()), priority(_priority) { // Matrices dimensions have to be consistent assert( A.rows() == b.rows() ); @@ -33,7 +33,7 @@ TaskBase::TaskBase(int _priority, const Eigen::MatrixXd& A, const soth::VectorBo constraint_matrix = A; bounds = b; -#ifdef DEBUG_MODE +#ifdef TASK_DEBUG INFO("Task base class initalized."); INFO("Constraint matrix:"); INFO(constraint_matrix); @@ -45,7 +45,8 @@ TaskBase::TaskBase(int _priority, const Eigen::MatrixXd& A, const soth::VectorBo /* * Overloaded version of the constructor. Takes as input the bound vector as a general Eigen::MatrixXd. */ -TaskBase::TaskBase(int _priority, const Eigen::MatrixXd& A, const Eigen::MatrixXd& b): priority(_priority) +TaskBase::TaskBase(int _priority, const Eigen::MatrixXd& A, const Eigen::MatrixXd& b): + boundType(soth::Bound::BOUND_DOUBLE), priority(_priority) { // Matrices dimensions have to be consistent assert( A.rows() == b.rows() ); @@ -57,7 +58,7 @@ TaskBase::TaskBase(int _priority, const Eigen::MatrixXd& A, const Eigen::MatrixX for(int i=0; iforward((&H_chain)); - H_chain = baseTransform * H_chain; + H_chain = parameters.baseT * H_chain; Eigen::VectorXd currentPose (constraint_matrix.rows()); if(constraint_matrix.rows() > POSITION_TASK_DIM) // Retrieving the position from the translation vector of the forward kinematics @@ -293,18 +348,19 @@ Eigen::VectorXd Task::getCurrentPose(const Eigen::Matrix4d& baseTransform) const /* TOCOMMENT */ const Eigen::VectorXd Task::getTargetPose() const { - if(positioningActive) - return path.at(path_currentStep); + if(parameters.positioningActive) + return parameters.path.at(parameters.path_currentStep); else return getCurrentPose(); } -void Task::setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints, const Eigen::Matrix4d& baseTransform) + +void Task::setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints) { // Re-computing direct kinematics Eigen::Matrix4d H_chain; theKinChain->forward((&H_chain)); - H_chain = baseTransform * H_chain; + H_chain = parameters.baseT * H_chain; Eigen::VectorXd initialPose (constraint_matrix.rows()); if(constraint_matrix.rows() > POSITION_TASK_DIM) @@ -318,21 +374,21 @@ void Task::setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints, const assert(dp.size() == initialPose.size()); - path.clear(); + parameters.path.clear(); for (float i = 1.0; i <= n_controlPoints; ++i) - path.push_back(initialPose + (i/static_cast(n_controlPoints)) * (dp - initialPose)); + parameters.path.push_back(initialPose + (i/static_cast(n_controlPoints)) * (dp - initialPose)); - path_currentStep = 0; - positioningActive = true; - if (jointControlActive) jointControlActive = false; + parameters.path_currentStep = 0; + parameters.positioningActive = true; + if (parameters.jointControlActive) parameters.jointControlActive = false; } -void Task::setDesiredPose(const Eigen::VectorXd& idp, const Eigen::VectorXd& dp, int n_controlPoints, const Eigen::Matrix4d& baseTransform) +void Task::setDesiredPose(const Eigen::VectorXd& idp, const Eigen::VectorXd& dp, int n_controlPoints) { // Re-computing direct kinematics Eigen::Matrix4d H_chain; theKinChain->forward((&H_chain)); - H_chain = baseTransform * H_chain; + H_chain = parameters.baseT * H_chain; Eigen::VectorXd initialPose (constraint_matrix.rows()); if(constraint_matrix.rows() > POSITION_TASK_DIM) @@ -347,16 +403,16 @@ void Task::setDesiredPose(const Eigen::VectorXd& idp, const Eigen::VectorXd& dp, assert(idp.size() == initialPose.size()); assert(dp.size() == initialPose.size()); - path.clear(); + parameters.path.clear(); for (float i = 1.0; i <= n_controlPoints; ++i) - path.push_back(initialPose + (i/static_cast(n_controlPoints)) * (idp - initialPose)); + parameters.path.push_back(initialPose + (i/static_cast(n_controlPoints)) * (idp - initialPose)); for (float i = 1.0; i <= n_controlPoints; ++i) - path.push_back(idp + (i/static_cast(n_controlPoints)) * (dp - idp)); + parameters.path.push_back(idp + (i/static_cast(n_controlPoints)) * (dp - idp)); - path_currentStep = 0; - positioningActive = true; - if (jointControlActive) jointControlActive = false; + parameters.path_currentStep = 0; + parameters.positioningActive = true; + if (parameters.jointControlActive) parameters.jointControlActive = false; } void Task::setDesiredConfiguration(const Eigen::VectorXd& desiredConf, int n_controlPoints) @@ -368,25 +424,24 @@ void Task::setDesiredConfiguration(const Eigen::VectorXd& desiredConf, int n_con constraint_matrix = Eigen::MatrixXd::Identity(initialConf.size(), initialConf.size()); bounds.setZero(initialConf.size(),1); - path.clear(); + parameters.path.clear(); for (float i = 1.0; i <= n_controlPoints; ++i) - path.push_back(initialConf + (i/static_cast(n_controlPoints)) * (desiredConf - initialConf)); + parameters.path.push_back(initialConf + (i/static_cast(n_controlPoints)) * (desiredConf - initialConf)); - path_currentStep = 0; - positioningActive = true; - if (!jointControlActive) jointControlActive = true; + parameters.path_currentStep = 0; + parameters.positioningActive = true; + if (!parameters.jointControlActive) parameters.jointControlActive = true; } -void Task::circularPathGenerator( const Eigen::VectorXd& dp, float z_shift, int n_controlPoints, float radius, int n, - const Eigen::Matrix4d& baseTransform ) +void Task::circularPathGenerator( const Eigen::VectorXd& dp, float z_shift, int n_controlPoints, float radius, int n ) { // Re-computing direct kinematics Eigen::Matrix4d H_chain; theKinChain->forward((&H_chain)); - H_chain = baseTransform * H_chain; + H_chain = parameters.baseT * H_chain; - path.clear(); + parameters.path.clear(); for (float i = 0.0; i < n*2*M_PI; i+= 2*M_PI/n_controlPoints) { Eigen::VectorXd ee_desiredPose_handframe(4); @@ -401,19 +456,19 @@ void Task::circularPathGenerator( const Eigen::VectorXd& dp, float z_shift, int Rmath::trim(&ee_desiredPose_CoMframe); - path.push_back( ee_desiredPose_CoMframe ); + parameters.path.push_back( ee_desiredPose_CoMframe ); } - path_currentStep = 0; - positioningActive = true; - if (jointControlActive) jointControlActive = false; + parameters.path_currentStep = 0; + parameters.positioningActive = true; + if (parameters.jointControlActive) parameters.jointControlActive = false; } /* * Task update function: update the constraint matrix A=A(q) with a new value of q and replace the target vector * with a proportional/derivative control law of the type K*POSE_ERROR + DESIRED_VELOCITY. */ -void Task::update( const Eigen::VectorXd& _q, const Eigen::VectorXd& desiredVel, double K, const Eigen::Matrix4d& baseTransform ) +void Task::update( const Eigen::VectorXd& _q, const Eigen::VectorXd& desiredVel, double K ) { // Dimensions must be consistent assert(constraint_matrix.rows() == desiredVel.size()); @@ -422,50 +477,57 @@ void Task::update( const Eigen::VectorXd& _q, const Eigen::VectorXd& desiredVel, Eigen::VectorXd q(_q.size()); q << -_q.head(base_end).reverse(), _q.tail(_q.size()-base_end); - Eigen::Matrix4d H_chain; - theKinChain->forward((&H_chain)); + // Updating the task kinematic chain with the new joint values + theKinChain->update(q); - // Cartesian space task - if (!jointControlActive) - { -#ifdef DEBUG_MODE +#ifdef TASK_DEBUG INFO("Updating task..."); + INFO("Current joint configuration: \n" << q); + INFO("Kinematic chain: \n" << (*theKinChain)); #endif - // Updating the task kinematic chain with the new joint values - theKinChain->update(q); - - // Re-computing differential kinematics - Eigen::MatrixXd J_chain(constraint_matrix.rows(), constraint_matrix.cols()); - theKinChain->differential(&J_chain,constraint_matrix.rows()); + // Cartesian space task + if (!parameters.jointControlActive) + { // Replacing the task constraint matrix with the updated version if (constraint_matrix.rows() > POSITION_TASK_DIM) { + // Re-computing differential kinematics + Eigen::MatrixXd J_chain(constraint_matrix.rows(), constraint_matrix.cols()); + theKinChain->differential(&J_chain); + // Computing base transform Eigen::MatrixXd baseT = Eigen::MatrixXd::Zero(6,6); - baseT.topLeftCorner(3,3) = baseTransform.topLeftCorner(3,3); - baseT.bottomRightCorner(3,3) = baseTransform.topLeftCorner(3,3); + baseT.topLeftCorner(3,3) = parameters.baseT.topLeftCorner(3,3); + baseT.bottomRightCorner(3,3) = parameters.baseT.topLeftCorner(3,3); // Pre-multipling base transform constraint_matrix << (baseT * J_chain).topRows(constraint_matrix.rows()); } else { + // Re-computing differential kinematics + Eigen::MatrixXd J_chain(constraint_matrix.rows(), constraint_matrix.cols()); + theKinChain->differential(&J_chain, POSITION_TASK_DIM); + // Pre-multipling base transform - constraint_matrix << (baseTransform.topLeftCorner(3,3) * J_chain).topRows(constraint_matrix.rows()); + constraint_matrix << (parameters.baseT.topLeftCorner(3,3) * J_chain).topRows(constraint_matrix.rows()); } -#ifdef DEBUG_MODE +#ifdef TASK_DEBUG INFO("New constraint matrix:"); - INFO(constraint_matrix); + INFO(std::endl << constraint_matrix); #endif + Eigen::VectorXd transitionVelocity(constraint_matrix.rows()); + transitionVelocity = constraint_matrix * parameters.qd_n; - if (positioningActive) + // Equality task with position control in the Cartesian space + if ( (parameters.positioningActive) && (bounds[0].getType() == soth::Bound::BOUND_TWIN) ) { // Re-computing direct kinematics Eigen::Matrix4d H_chain; theKinChain->forward((&H_chain)); - H_chain = baseTransform * H_chain; + H_chain = parameters.baseT * H_chain; // Computing the current pose in the task space Eigen::VectorXd currentPose(constraint_matrix.rows()); @@ -480,76 +542,112 @@ void Task::update( const Eigen::VectorXd& _q, const Eigen::VectorXd& desiredVel, currentPose << H_chain.col(POSITION_TASK_DIM).head(constraint_matrix.rows()); Eigen::VectorXd pose_error; - if(path_currentStep < path.size()) - pose_error = path.at(path_currentStep) - currentPose; + if(parameters.path_currentStep < parameters.path.size()) + pose_error = parameters.path.at(parameters.path_currentStep) - currentPose; else - pose_error = path.at(path.size()-1) - currentPose; + pose_error = parameters.path.at(parameters.path.size()-1) - currentPose; + + parameters.targetVelocity = K * pose_error + desiredVel; // Updating the bound vector with the task error + a feedforward term for(int i=0; iforward((&H_chain)); + H_chain = parameters.baseT * H_chain; + + // Computing the current pose in the task space + Eigen::VectorXd currentPose(constraint_matrix.rows()); + // If the task target is a pose (position+orientation) vector, a minimal description of the orientation has to be computed + if(constraint_matrix.rows() > POSITION_TASK_DIM) + // Retrieving the position from the translation vector of the forward kinematics + // Retrieving the orientation from the Euler fixed frame x-y-z angles + currentPose << H_chain.topRightCorner(POSITION_TASK_DIM,1), + Rmath::xyzEulerAngles( H_chain.topLeftCorner(3,3) ).head(constraint_matrix.rows()-POSITION_TASK_DIM); + // If the task target is a position vector (A row size <= 3) just the translation vector of the forward kinematics is needed + else + currentPose << H_chain.col(POSITION_TASK_DIM).head(constraint_matrix.rows()); + + Eigen::VectorXd new_b (bounds.rows()); + new_b = ( parameters.path.at(parameters.path.size()-1) - currentPose ) / TIME_STEP; for(int i=0; iupdate(q); - - if (positioningActive) + // Equality task with position control in the joint space + if ( (parameters.positioningActive) && (bounds[0].getType() == soth::Bound::BOUND_TWIN) ) { Eigen::VectorXd joint_error; - if(path_currentStep < path.size()) - joint_error = path.at(path_currentStep) - theKinChain->jointConfiguration(); + if(parameters.path_currentStep < parameters.path.size()) + joint_error = parameters.path.at(parameters.path_currentStep) - theKinChain->jointConfiguration(); else - joint_error = path.at(path.size()-1) - theKinChain->jointConfiguration(); + joint_error = parameters.path.at(parameters.path.size()-1) - theKinChain->jointConfiguration(); + parameters.targetVelocity = K * joint_error + desiredVel; // Updating the bound vector with the task error + a feedforward term for(int i=0; i +#include + +#include "r2Module.h" + +#define INFO(x) std::cerr << "\033[22;34;1m" << "[TaskManager] " << x << "\033[0m" << std::endl; + +using namespace AL; + +void R2Module::TaskManager::taskUpdate(const std::string& taskName, const Eigen::VectorXd& q) +{ + if( taskName == HEAD_TASK) + { + // Head task update + Eigen::VectorXd q_H (LLEG_CHAIN_SIZE+HEAD_CHAIN_SIZE); + q_H << q.segment(LLEG_CHAIN_BEGIN), + q.segment(HEAD_CHAIN_BEGIN); + ( taskMap[taskName] )->update( q_H, Eigen::VectorXd::Zero(HEAD_TASK_DIM), K_HEAD ); + +#ifdef DEBUG_MODE + INFO("Head task constraint equation: "); + INFO(std::endl << *( taskMap[taskName] ) ); +#endif + } + + else if( (taskName == LEFT_ARM) ) + { + // Left arm task update + Eigen::VectorXd q_LA (LLEG_CHAIN_SIZE+LARM_CHAIN_SIZE); + q_LA << q.segment(LLEG_CHAIN_BEGIN), + q.segment(LARM_CHAIN_BEGIN); + ( taskMap[taskName] )->update( q_LA, Eigen::VectorXd::Zero(LARM_TASK_DIM), K_LARM ); + +#ifdef DEBUG_MODE + INFO("Left arm task constraint equation: "); + INFO(std::endl << *( taskMap[taskName] ) ); +#endif + } + + else if( (taskName == RIGHT_ARM) ) + { + // Right arm task update + Eigen::VectorXd q_RA (LLEG_CHAIN_SIZE+RARM_CHAIN_SIZE); + q_RA << q.segment(LLEG_CHAIN_BEGIN), + q.segment(RARM_CHAIN_BEGIN); +#ifdef RARM_LARM_JOINT_TASK + if (taskName == "Rbound") + ( taskMap[taskName] )->update( q_RA, Eigen::VectorXd::Zero(RARM_TASK_DIM), K_RARM ); + // Right arm mimic task + else if ( ARMS_TASK == MIMIC_TASK ) + { + Eigen::VectorXd desiredRHandPose = ( taskMap[LEFT_ARM] )->getTargetPose(); + assert(RARM_TASK_DIM == LARM_TASK_DIM); + assert(LARM_TASK_DIM >= 2); + desiredRHandPose(1) -= MINIMUM_HANDS_DISTANCE; + + if (LARM_TASK_DIM > 3) + for (int i=3; i < LARM_TASK_DIM; ++i) + desiredRHandPose(i) = -desiredRHandPose(i); + + ( taskMap[taskName] )->setDesiredPose(desiredRHandPose, 1 ); + ( taskMap[taskName] )->update( q_RA, Eigen::VectorXd::Zero(RARM_TASK_DIM), K_RARM ); + + } + // Right arm mirror task + else if ( ARMS_TASK == MIRROR_TASK ) + { + Eigen::VectorXd desiredRHandVel = ( taskMap[LEFT_ARM] )->getTargetVelocity(); + + assert(RARM_TASK_DIM == LARM_TASK_DIM); + assert(LARM_TASK_DIM >= 2); + desiredRHandVel(1) = -desiredRHandVel(1); + + if (LARM_TASK_DIM > 3) + for (int i=3; i < LARM_TASK_DIM; ++i) + desiredRHandVel(i) = -desiredRHandVel(i); + + ( taskMap[taskName] )->update( q_RA, desiredRHandVel, K_RARM ); + } +#else + ( taskMap[taskName] )->update( q_RA, Eigen::VectorXd::Zero(RARM_TASK_DIM), K_RARM ); +#endif + +#ifdef DEBUG_MODE + INFO("Right arm task constraint equation: "); + INFO(std::endl << *( taskMap[taskName] ) ); +#endif + } + + else if( (taskName == RIGHT_LEG) ) + { + // Right leg task update + ( taskMap[taskName] )->update( q.segment(RLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(RLEG_TASK_DIM), K_RLEG ); + +#ifdef DEBUG_MODE + INFO("Right leg task constraint equation: "); + INFO(std::endl << *( taskMap[taskName] ) ); +#endif + } + + else if( (taskName == LEFT_LEG) ) + { + // Left leg task update + ( taskMap[taskName] )->update( q.segment(LLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(LLEG_TASK_DIM), K_LLEG ); + +#ifdef DEBUG_MODE + INFO("Left leg task constraint equation: "); + INFO(std::endl << *( taskMap[taskName] ) ); +#endif + } +} + +void R2Module::TaskManager::updateActiveTasks(const Eigen::VectorXd& q) +{ + std::map::iterator updater=taskMap.begin(); + while(updater != taskMap.end()) + { + if( (updater->second)->taskStatus() == active ) + { + taskUpdate(updater->first, q ); + taskSet.insert(updater->second); + } + + ++updater; + } +} + +void R2Module::TaskManager::updateIntermediateTasks(const Eigen::VectorXd& q, const Eigen::VectorXd& partial_qdot ) +{ + std::map::iterator updater=taskMap.begin(); + while(updater != taskMap.end()) + { + if( (updater->second)->taskStatus() == inactive2active || (updater->second)->taskStatus() == active2inactive ) + { + // Retrieve current kinchain joints information + std::map jointsIndicesMap; + (updater->second)->getJointsIDs(&jointsIndicesMap); + Eigen::VectorXd current_qd((updater->second)->constraintMatrix().cols()); + + for (unsigned int i = 0; i < module_ptr->jointID.size(); ++i) + if (jointsIndicesMap.find(module_ptr->jointID.at(i)) != jointsIndicesMap.end()) + current_qd(jointsIndicesMap[module_ptr->jointID.at(i)]) = partial_qdot(i); + + // qd_n <- cropped qdot + (updater->second)->set_qd(current_qd); + + taskUpdate(updater->first, q ); + taskSet.insert(updater->second); + } + + ++updater; + } +} + +void R2Module::TaskManager::computePartialSolution(const Eigen::VectorXd& q, Eigen::VectorXd* partial_qdot) +{ + assert(partial_qdot->size() == q.size()); + + updateActiveTasks(q); + + // Initialize the HQP solver + soth::HCOD hsolver( q.size(), taskSet.size()+1 ); + + // Set up the hierarchy + std::vector A; + std::vector b; + // Joint limits first (top priority task) + A.push_back(jointLimits->constraintMatrix()); + b.push_back(jointLimits->vectorBounds()); + // All the remaining tasks (in descending order of priority) + std::set::iterator updater = taskSet.begin(); + while( updater != taskSet.end() ) + { + // Retrieve current kinchain joints information + std::map jointsIndicesMap; + (*updater)->getJointsIDs(&jointsIndicesMap); + // Current task constraint matrix (restricted to the joint involved) + Eigen::MatrixXd currentA_task = (*updater)->constraintMatrix(); + + // Rewrite the task constraint matrix with the complete joint configuration + Eigen::MatrixXd currentA = Eigen::MatrixXd::Zero( currentA_task.rows(), JOINTS_NUM ); + // The task constraint matrix is the only non-zero block in A + for (unsigned int i = 0; i jointID.size(); ++i) + { + if (jointsIndicesMap.find(module_ptr->jointID.at(i)) != jointsIndicesMap.end()) + currentA.col(i) = currentA_task.col( jointsIndicesMap[module_ptr->jointID.at(i)] ); + else + currentA.col(i) = Eigen::VectorXd::Zero(currentA_task.rows()); + } + + A.push_back(currentA); + b.push_back((*updater)->vectorBounds()); + ++updater; + } + + hsolver.pushBackStages(A, b); + + // Configure the solver + hsolver.setDamping(0.0); + hsolver.setInitialActiveSet(); + + // Run the solver + hsolver.activeSearch(*partial_qdot); + // Trim solution + Rmath::trim(partial_qdot); +} + +void R2Module::TaskManager::computeCompleteSolution(const Eigen::VectorXd& q, const Eigen::VectorXd& partial_qdot, Eigen::VectorXd* qdot) +{ + assert(partial_qdot.size() == q.size()); + assert(qdot->size() == q.size()); + + updateIntermediateTasks(q, partial_qdot); + + // Initialize the HQP solver + soth::HCOD hsolver( q.size(), taskSet.size()+1 ); +#ifdef DEBUG_MODE + INFO("Initializing HQP solver..."); + INFO( "Active tasks: " << (taskSet.size()+1) << " out of " << (taskMap.size()+1) ); + INFO("Generating stack..."); + + int task_i = 2; +#endif + // Set up the hierarchy + std::vector A; + std::vector b; + // Joint limits first (top priority task) + A.push_back(jointLimits->constraintMatrix()); + b.push_back(jointLimits->vectorBounds()); + // All the remaining tasks (in descending order of priority) + std::set::iterator updater = taskSet.begin(); + while( updater != taskSet.end() ) + { + // Retrieve current kinchain joints information + std::map jointsIndicesMap; + (*updater)->getJointsIDs(&jointsIndicesMap); + // Current task constraint matrix (restricted to the joint involved) + Eigen::MatrixXd currentA_task = (*updater)->constraintMatrix(); + + // Rewrite the task constraint matrix with the complete joint configuration + Eigen::MatrixXd currentA = Eigen::MatrixXd::Zero( currentA_task.rows(), JOINTS_NUM ); + // The task constraint matrix is the only non-zero block in A + for (unsigned int i = 0; i jointID.size(); ++i) + { + if (jointsIndicesMap.find(module_ptr->jointID.at(i)) != jointsIndicesMap.end()) + currentA.col(i) = currentA_task.col( jointsIndicesMap[module_ptr->jointID.at(i)] ); + else + currentA.col(i) = Eigen::VectorXd::Zero(currentA_task.rows()); + } + + A.push_back(currentA); + b.push_back((*updater)->vectorBounds()); + ++updater; + +#ifdef DEBUG_MODE + INFO("Task n. " << task_i << ", constraint matrix (enlarged):\n" << currentA); + ++task_i; +#endif + } + + hsolver.pushBackStages(A, b); + +#ifdef DEBUG_MODE + INFO("Configuring HQP solver..."); + INFO("Start active search... "); +#endif + + // Configure the solver + hsolver.setDamping(0.0); + hsolver.setInitialActiveSet(); + + // Run the solver + hsolver.activeSearch(*qdot); + // Trim solution + Rmath::trim(qdot); + +#ifdef DEBUG_MODE + INFO("Done."); + INFO("Active set: "); + hsolver.showActiveSet(std::cerr); + INFO("Solution: [\n" << *qdot << "]"); +#endif + +} + + +R2Module::TaskManager::~TaskManager() +{ + // Deallocate memory + if (jointLimits) delete jointLimits; + + // Cleaning up containers + std::map::iterator destroyer = taskMap.begin(); + while (destroyer != taskMap.end()) + { + if(destroyer->second) delete destroyer->second; + ++destroyer; + } + taskMap.clear(); + taskSet.clear(); +} + +void R2Module::TaskManager::setJointLimits(const Eigen::MatrixXd& velBounds) +{ + // Dimensions check + assert(velBounds.rows() == JOINTS_NUM); + assert(velBounds.cols() == 2); + + // Replace current TaskBase* + if (jointLimits) delete jointLimits; + jointLimits = new TaskBase(0, Eigen::MatrixXd::Identity( JOINTS_NUM, JOINTS_NUM ), velBounds); +} + +void R2Module::TaskManager::exec(const Eigen::VectorXd& q, Eigen::VectorXd* qdot) +{ + // Dimension check + assert(qdot->size() == JOINTS_NUM); + +#ifdef TASK_MANAGER_DEBUG + INFO("-----------------------"); + std::map::const_iterator watcher = taskMap.begin(); + while(watcher != taskMap.end()) + { + if ((watcher->first == "LJesus") || (watcher->first == "LMary")) + { + INFO(watcher->first << ": "); + INFO("Status = " << (watcher->second)->taskStatus()); + INFO("Priority = " << (watcher->second)->getPriority()); + INFO("Activation value = " << (watcher->second)->activationValue()); + if ((watcher->second)->done()) + { + INFO("Task done."); + } + else + { + INFO("Task in progress..."); + } + } + ++watcher; + } +#endif + + // Handle tasks waiting for a priority change + std::map::iterator priorityHandler = changingTaskMap.begin(); + while(priorityHandler != changingTaskMap.end()) + { + Task* currentTask = taskMap[priorityHandler->first]; + + // The task finally stopped and has to be re-activated with the new priority value + if ( (currentTask->getPriority() != priorityHandler->second) && (currentTask->taskStatus() == inactive) ) + { + // Set the task priority to the new value + currentTask->setPriority( priorityHandler->second ); + // Insert the task back + currentTask->activate(); + // Remove the task from the queue + changingTaskMap.erase(priorityHandler); + } + // The task is still active with the old priority + else if ( (currentTask->getPriority() != priorityHandler->second) && (currentTask->taskStatus() != active2inactive) ) + currentTask->stop(); + + ++priorityHandler; + } + + // Update joint limits task + Eigen::MatrixXd velBounds(q.size(), 2); + module_ptr->posBound2velBound(module_ptr->jointBounds, q, &velBounds); + jointLimits->setVectorBounds(velBounds); + +#ifdef DEBUG_MODE + INFO("Updating joint bounds: "); + INFO(std::endl << *jointLimits); +#endif + + taskSet.clear(); + // Compute the partial solution first + Eigen::VectorXd partialSolution(q.size()); + computePartialSolution(q, &partialSolution); + + // Compute the final solution + computeCompleteSolution(q, partialSolution, qdot); +} From ace83da0311ce3dc1ebf7cfb8fa9aaf3d4311a31 Mon Sep 17 00:00:00 2001 From: Francesco Riccio Date: Tue, 28 Jan 2014 17:49:56 +0100 Subject: [PATCH 6/6] R2 --- naoqi/modules/R2module/CMakeLists.txt | 2 + naoqi/modules/R2module/include/configParams.h | 28 ++- naoqi/modules/R2module/include/task.h | 9 + .../R2module/soth-master/src/Stage.cpp | 2 +- naoqi/modules/R2module/src/r2Module.cpp | 201 ++++++++++++++++-- naoqi/modules/R2module/src/task.cpp | 3 - naoqi/modules/R2module/src/taskManager.cpp | 132 ++++++++++-- 7 files changed, 335 insertions(+), 42 deletions(-) diff --git a/naoqi/modules/R2module/CMakeLists.txt b/naoqi/modules/R2module/CMakeLists.txt index b6c4f47..5165470 100644 --- a/naoqi/modules/R2module/CMakeLists.txt +++ b/naoqi/modules/R2module/CMakeLists.txt @@ -19,12 +19,14 @@ set(_srcs ${include_path}/libmath/Rutils.h ${include_path}/task.h ${include_path}/r2Module.h + ${include_path}/configParams.h ${src_path}/configReader.cpp ${src_path}/libmath/transform.cpp ${src_path}/libmath/kinChain.cpp ${src_path}/libmath/Rutils.cpp ${src_path}/task.cpp + ${src_path}/taskManager.cpp ${src_path}/r2ModuleMain.cpp ${src_path}/r2Module.cpp diff --git a/naoqi/modules/R2module/include/configParams.h b/naoqi/modules/R2module/include/configParams.h index 390420a..25cbfd1 100644 --- a/naoqi/modules/R2module/include/configParams.h +++ b/naoqi/modules/R2module/include/configParams.h @@ -6,6 +6,7 @@ //#define DEBUG_MODE //#define TASK_DEBUG //#define TASK_MANAGER_DEBUG +#define LOG // Discrete integration time step @@ -94,7 +95,7 @@ static Eigen::Matrix4d base_ankle; /** ---------------------------- Task parameters ---------------------------- */ -static const float ACTIVATION_STEP = 1.00; +static const float ACTIVATION_STEP = 0.1; // Task names static const std::string HEAD_TASK = "Head task"; @@ -121,13 +122,13 @@ static const int LLEG_TASK_PRIORITY = 1; static const double K_HEAD = 1.0; static const double K_RARM = 1.0; static const double K_LARM = 1.0; -static const double K_RLEG = 0.8; -static const double K_LLEG = 0.8; +static const double K_RLEG = 0.6; +static const double K_LLEG = 0.6; // Control points number static const int HEAD_TASK_NSTEPS = 1; static const int RARM_TASK_NSTEPS = 15; -static const int LARM_TASK_NSTEPS = 8; +static const int LARM_TASK_NSTEPS = 30; static const int RLEG_TASK_NSTEPS = 30; static const int LLEG_TASK_NSTEPS = 30; @@ -138,7 +139,7 @@ static const double LARM_TRANSITION_STEP = 1.0; static const double RLEG_TRANSITION_STEP = 1.0; static const double LLEG_TRANSITION_STEP = 1.0; -//#define RARM_LARM_JOINT_TASK +#define RARM_LARM_JOINT_TASK enum TaskType { MIRROR_TASK, MIMIC_TASK @@ -147,7 +148,7 @@ static const TaskType ARMS_TASK = MIRROR_TASK; static const double MINIMUM_HANDS_DISTANCE = 100.0; // Circle trajectory task parameters -//#define LARM_CIRCLE_TASK +#define LARM_CIRCLE_TASK //#define RARM_CIRCLE_TASK static const double CIRCLE_Z_DEPTH = -70.0; // wrt ee-frame static const double CIRCLE_RADIUS = 80.0; @@ -158,9 +159,9 @@ static const int CIRCLE_LAPS = 10; static int UP_DOWN = 80.0; - /** ------------------------------- Desired poses ------------------------------- */ + static Eigen::VectorXd desiredHeadPose(6); static Eigen::VectorXd desiredRHandPose(6); static Eigen::VectorXd desiredLHandPose(6); @@ -230,6 +231,19 @@ static void initialization() RLEG_DESIRED_ROLL, RLEG_DESIRED_PITCH, RLEG_DESIRED_YAW; desiredLLegPose << LLEG_DESIRED_X, LLEG_DESIRED_Y, LLEG_DESIRED_Z, LLEG_DESIRED_ROLL, LLEG_DESIRED_PITCH, LLEG_DESIRED_YAW; + + + /*TOFIX*/ + assert(HEAD_TASK_DIM <=6); + desiredHeadPose = desiredHeadPose.head(HEAD_TASK_DIM); + assert(RARM_TASK_DIM <=6); + desiredRHandPose = desiredRHandPose.head(RARM_TASK_DIM); + assert(LARM_TASK_DIM <= 6); + desiredLHandPose = desiredLHandPose.head(LARM_TASK_DIM); + assert(RLEG_TASK_DIM <= 6); +// desiredRLegPose = desiredRLegPose.head(RLEG_TASK_DIM); + assert(LLEG_TASK_DIM <= 6); +// desiredLLegPose = desiredLLegPose.head(LLEG_TASK_DIM); } #endif diff --git a/naoqi/modules/R2module/include/task.h b/naoqi/modules/R2module/include/task.h index cbf1cac..7e5d7b6 100644 --- a/naoqi/modules/R2module/include/task.h +++ b/naoqi/modules/R2module/include/task.h @@ -180,6 +180,8 @@ class Task : public TaskBase void activate(float activationStep = ACTIVATION_STEP); void stop( float decayStep = ACTIVATION_STEP); + inline float activationValue(){ return parameters.activationValue; } + void set_qd(Eigen::VectorXd new_qd) { assert(new_qd.size() == parameters.qd_n.size()); @@ -220,6 +222,13 @@ class Task : public TaskBase return (parameters.path_currentStep == parameters.path.size()-1); } + // Set a desired velocity in the task space + inline void setTargetVelocity(const Eigen::VectorXd& r) + { + assert(r.size() == parameters.targetVelocity.size()); + parameters.targetVelocity = r; + parameters.positioningActive = false; + } // Set a desired pose in the task space void setDesiredPose( const Eigen::VectorXd& dp, int n_controlPoints = 1 ); void setDesiredPose( const Eigen::VectorXd& idp, const Eigen::VectorXd& dp, int n_controlPoints = 1 ); diff --git a/naoqi/modules/R2module/soth-master/src/Stage.cpp b/naoqi/modules/R2module/soth-master/src/Stage.cpp index 84504cf..38c2464 100644 --- a/naoqi/modules/R2module/soth-master/src/Stage.cpp +++ b/naoqi/modules/R2module/soth-master/src/Stage.cpp @@ -548,7 +548,7 @@ namespace soth G1.transpose() >> M; G1.transpose() >> Ln; W << G1; - assert( std::abs(ML_(Irn(j),sizeM))getPriority()); @@ -217,6 +251,7 @@ void R2Module::init() INFO("- Left leg task, with priority " << Lsupporting->getPriority()); #endif } + } /* @@ -296,7 +331,7 @@ void R2Module::getCurrentFrame() if ((char) cv::waitKey(33) != 27) cv::imshow("Frame", fcurrImageHeader); - // Release the proxystd::cout<<"jesus: \n"<releaseImage(fVideoClientName); } @@ -343,6 +378,25 @@ void R2Module::motion() taskManager.task(RIGHT_LEG).activate(RLEG_TRANSITION_STEP); taskManager.task(LEFT_LEG).activate(LLEG_TRANSITION_STEP); + /****/ +// taskManager.task("rleg1").activate(1.0); +// taskManager.task("lleg1").activate(1.0); +// taskManager.task("rleg2").activate(1.0); +// taskManager.task("lleg2").activate(1.0); + +// taskManager.task("jesus").stop(1.0); + + taskManager.task("Rbound").stop(1.0); + +// taskManager.task("LJesus").activate(); +// taskManager.task("LMary").activate(); +// taskManager.task("LHolySpirit").activate(); +// taskManager.task("RJesus").activate(); +// taskManager.task("RMary").activate(); +// taskManager.task("RHolySpirit").activate(); + + /****/ + Eigen::Matrix4d h_CoMRL, h_CoMLL; CoM_RightLeg->forward(&h_CoMRL); CoM_LeftLeg->forward(&h_CoMLL); @@ -354,29 +408,85 @@ void R2Module::motion() taskManager.task(LEFT_ARM).set_baseTransform(base_ankle); taskManager.task(RIGHT_ARM).set_baseTransform(base_ankle); + /****/ +// taskManager.task("rleg1").set_baseTransform(h_CoMRL); +// taskManager.task("lleg1").set_baseTransform(h_CoMLL); +// taskManager.task("rleg2").set_baseTransform(h_CoMRL); +// taskManager.task("lleg2").set_baseTransform(h_CoMLL); + + taskManager.task("Rbound").set_baseTransform(base_ankle); + +// taskManager.task("jesus").set_baseTransform(base_ankle); + +// taskManager.task("LJesus").set_baseTransform(base_ankle); +// taskManager.task("LMary").set_baseTransform(base_ankle); +// taskManager.task("LHolySpirit").set_baseTransform(base_ankle); +// taskManager.task("RJesus").set_baseTransform(base_ankle); +// taskManager.task("RMary").set_baseTransform(base_ankle); +// taskManager.task("RHolySpirit").set_baseTransform(base_ankle); + +// taskManager.task("LJesus").setTargetVelocity(Eigen::Vector3d::UnitX()*50); +// taskManager.task("LMary").setTargetVelocity(Eigen::Vector3d::UnitZ()*50); +// taskManager.task("LHolySpirit").setTargetVelocity(Eigen::Vector3d::UnitY()*-50); +// taskManager.task("RJesus").setTargetVelocity(Eigen::Vector3d::UnitX()*50); +// taskManager.task("RMary").setTargetVelocity(Eigen::Vector3d::UnitZ()*-50); +// taskManager.task("RHolySpirit").setTargetVelocity(Eigen::Vector3d::UnitY()*-50); + + // Tasks rleg1-2, lleg1-2 +// Eigen::VectorXd desiredRLegPose2 = desiredRLegPose; +// desiredRLegPose2(2) += 100.0; +// Eigen::VectorXd desiredLLegPose2 = desiredLLegPose; +// desiredLLegPose2(2) += 100.0; +// taskManager.task("rleg1").setDesiredPose(desiredRLegPose, 1); +// taskManager.task("lleg1").setDesiredPose(desiredLLegPose, 1); +// taskManager.task("rleg2").setDesiredPose(desiredRLegPose2, 1); +// taskManager.task("lleg2").setDesiredPose(desiredLLegPose2, 1); + + // Task jesus +// Eigen::VectorXd desiredRA2pose(3); +// desiredRA2pose << 0.0, desiredRHandPose(1), 618.0; +// taskManager.task("jesus").setDesiredPose(desiredRA2pose, RARM_TASK_NSTEPS); + + // Task Rbound + Eigen::VectorXd desiredRHandPose2 = desiredRHandPose; + desiredRHandPose2(0) = -Eigen::Infinity; + desiredRHandPose2(1) = RARM_DESIRED_Y; + desiredRHandPose2(2) = -Eigen::Infinity; + taskManager.task("Rbound").setDesiredPose(desiredRHandPose2); + + /****/ + +#ifndef UP_DOWN_TASK + if(taskManager.task(RIGHT_LEG).taskStatus() != inactive) + taskManager.task(RIGHT_LEG).setDesiredPose( desiredRLegPose, RLEG_TASK_NSTEPS ); + + if(taskManager.task(LEFT_LEG).taskStatus() != inactive) + taskManager.task(LEFT_LEG).setDesiredPose( desiredLLegPose, LLEG_TASK_NSTEPS ); +#else if(taskManager.task(RIGHT_LEG).taskStatus() != inactive) - taskManager.task(RIGHT_LEG).setDesiredPose( desiredRLegPose.head(RLEG_TASK_DIM), RLEG_TASK_NSTEPS ); + taskManager.task(RIGHT_LEG).setDesiredPose( desiredRLegPose, 5 ); if(taskManager.task(LEFT_LEG).taskStatus() != inactive) - taskManager.task(LEFT_LEG).setDesiredPose( desiredLLegPose.head(LLEG_TASK_DIM), LLEG_TASK_NSTEPS ); + taskManager.task(LEFT_LEG).setDesiredPose( desiredLLegPose, 5 ); +#endif if(taskManager.task(HEAD_TASK).taskStatus() != inactive) - taskManager.task(HEAD_TASK).setDesiredPose( desiredHeadPose.head(HEAD_TASK_DIM), HEAD_TASK_NSTEPS ); + taskManager.task(HEAD_TASK).setDesiredPose( desiredHeadPose, HEAD_TASK_NSTEPS ); if(taskManager.task(LEFT_ARM).taskStatus() != inactive) #ifndef LARM_CIRCLE_TASK - taskManager.task(LEFT_ARM).setDesiredPose( desiredLHandPose.head(LARM_TASK_DIM), LARM_TASK_NSTEPS ); + taskManager.task(LEFT_ARM).setDesiredPose( desiredLHandPose, LARM_TASK_NSTEPS ); #else - taskManager.task(LEFT_ARM).circularPathGenerator(desiredLHandPose.head(LARM_TASK_DIM), CIRCLE_Z_DEPTH, + taskManager.task(LEFT_ARM).circularPathGenerator(desiredLHandPose, CIRCLE_Z_DEPTH, LARM_TASK_NSTEPS, CIRCLE_RADIUS, CIRCLE_LAPS ); #endif #ifndef RARM_LARM_JOINT_TASK if(taskManager.task(RIGHT_ARM).taskStatus() != inactive) #ifndef RARM_CIRCLE_TASK - taskManager.task(RIGHT_ARM).setDesiredPose(desiredRHandPose.head(RARM_TASK_DIM), RARM_TASK_NSTEPS ); + taskManager.task(RIGHT_ARM).setDesiredPose(desiredRHandPose, RARM_TASK_NSTEPS ); #else - taskManager.task(RIGHT_ARM).circularPathGenerator(desiredRHandPose.head(RARM_TASK_DIM), CIRCLE_Z_DEPTH, + taskManager.task(RIGHT_ARM).circularPathGenerator(desiredRHandPose, CIRCLE_Z_DEPTH, RARM_TASK_NSTEPS, CIRCLE_RADIUS, CIRCLE_LAPS ); #endif #endif @@ -390,28 +500,85 @@ void R2Module::motion() #endif INFO("Desired right foot position in space: [\n" << desiredRLegPose << "]"); INFO("Desired Hip position in space: [\n" << desiredLLegPose << "]"); + +#endif +#ifdef UP_DOWN_TASK + int updown_counter = 0; + int mary_holyspirit_flag = 1; #endif + int counter = 0; // Main loop while(true) { +#ifdef LOG + ++counter; + INFO("----------------------- iteration: "<(LLEG_CHAIN_BEGIN), q.segment(LARM_CHAIN_BEGIN); ( taskMap[taskName] )->update( q_LA, Eigen::VectorXd::Zero(LARM_TASK_DIM), K_LARM ); -#ifdef DEBUG_MODE - INFO("Left arm task constraint equation: "); - INFO(std::endl << *( taskMap[taskName] ) ); -#endif } - - else if( (taskName == RIGHT_ARM) ) +// else if( (taskName == "LHolySpirit") || (taskName == "LJesus") || (taskName == "LMary") ) +// { +// // Left arm task update +// Eigen::VectorXd q_LA (LARM_CHAIN_SIZE); +// q_LA << q.segment(LARM_CHAIN_BEGIN); +// ( taskMap[taskName] )->update( q_LA, Eigen::VectorXd::Zero(LARM_TASK_DIM), K_LARM ); + +//#ifdef DEBUG_MODE +// INFO("Left arm task constraint equation: "); +// INFO(std::endl << *( taskMap[taskName] ) ); +//#endif +// } +// else if( (taskName == "RHolySpirit") || (taskName == "RJesus") || (taskName == "RMary") ) +// { +// // Left arm task update +// Eigen::VectorXd q_RA (RARM_CHAIN_SIZE); +// q_RA << q.segment(RARM_CHAIN_BEGIN); +// ( taskMap[taskName] )->update( q_RA, Eigen::VectorXd::Zero(RARM_TASK_DIM), K_RARM ); + +//#ifdef DEBUG_MODE +// INFO("Right arm task constraint equation: "); +// INFO(std::endl << *( taskMap[taskName] ) ); +//#endif +// } + + else if( (taskName == RIGHT_ARM) || (taskName == "Rbound") ) { // Right arm task update Eigen::VectorXd q_RA (LLEG_CHAIN_SIZE+RARM_CHAIN_SIZE); q_RA << q.segment(LLEG_CHAIN_BEGIN), q.segment(RARM_CHAIN_BEGIN); + #ifdef RARM_LARM_JOINT_TASK if (taskName == "Rbound") ( taskMap[taskName] )->update( q_RA, Eigen::VectorXd::Zero(RARM_TASK_DIM), K_RARM ); @@ -83,6 +104,7 @@ void R2Module::TaskManager::taskUpdate(const std::string& taskName, const Eigen: for (int i=3; i < LARM_TASK_DIM; ++i) desiredRHandVel(i) = -desiredRHandVel(i); + ( taskMap[taskName] )->setTargetVelocity(desiredRHandVel); ( taskMap[taskName] )->update( q_RA, desiredRHandVel, K_RARM ); } #else @@ -94,23 +116,32 @@ void R2Module::TaskManager::taskUpdate(const std::string& taskName, const Eigen: INFO(std::endl << *( taskMap[taskName] ) ); #endif } - +// else if ( (taskName == "rleg1") || (taskName == "rleg2") ) +// ( taskMap[taskName] )->update( q.segment(RLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(RLEG_TASK_DIM), K_RLEG ); else if( (taskName == RIGHT_LEG) ) { // Right leg task update +#ifndef UP_DOWN_TASK ( taskMap[taskName] )->update( q.segment(RLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(RLEG_TASK_DIM), K_RLEG ); +#else + ( taskMap[taskName] )->update( q.segment(RLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(RLEG_TASK_DIM), 0.4 ); +#endif #ifdef DEBUG_MODE INFO("Right leg task constraint equation: "); INFO(std::endl << *( taskMap[taskName] ) ); #endif } - +// else if ( (taskName == "lleg1") || (taskName == "lleg2") ) +// ( taskMap[taskName] )->update( q.segment(LLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(LLEG_TASK_DIM), K_LLEG ); else if( (taskName == LEFT_LEG) ) { +#ifndef UP_DOWN_TASK // Left leg task update ( taskMap[taskName] )->update( q.segment(LLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(LLEG_TASK_DIM), K_LLEG ); - +#else + ( taskMap[taskName] )->update( q.segment(RLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(LLEG_TASK_DIM), 0.4 ); +#endif #ifdef DEBUG_MODE INFO("Left leg task constraint equation: "); INFO(std::endl << *( taskMap[taskName] ) ); @@ -329,7 +360,7 @@ void R2Module::TaskManager::exec(const Eigen::VectorXd& q, Eigen::VectorXd* qdot std::map::const_iterator watcher = taskMap.begin(); while(watcher != taskMap.end()) { - if ((watcher->first == "LJesus") || (watcher->first == "LMary")) +// if ((watcher->first == "Rbound") || (watcher->first == RIGHT_ARM) ) { INFO(watcher->first << ": "); INFO("Status = " << (watcher->second)->taskStatus()); @@ -388,4 +419,77 @@ void R2Module::TaskManager::exec(const Eigen::VectorXd& q, Eigen::VectorXd* qdot // Compute the final solution computeCompleteSolution(q, partialSolution, qdot); + +#ifdef LOG + Eigen::VectorXd qdot_LOG(JOINTS_NUM); + qdot_LOG = *qdot; + std::map::const_iterator logger = taskMap.begin(); + while(logger != taskMap.end()) + { + if((logger->first == LEFT_ARM)) + { + INFO("-------- task name: "<< logger->first <<" --------"); + + INFO("task status: " << (logger->second)->taskStatus() ); + INFO("priority: " << (logger->second)->getPriority()); + INFO("activation function: " << (logger->second)->activationValue()); + + Eigen::Vector3d ee_position_LOG; + Eigen::Matrix4d h_LOG; + ((logger->second)->kinChain())->forward(&h_LOG); + ee_position_LOG = h_LOG.topRightCorner(3,1); + + Eigen::Vector3d velocity; + Eigen::Vector3d velocityError_LOG; + Eigen::Vector3d velocityTarget_LOG; + velocityTarget_LOG = (logger->second)->getTargetVelocity(); + Eigen::MatrixXd j_LOG; + ((logger->second)->kinChain())->differential(&j_LOG); + + Eigen::VectorXd qdot_LA (LLEG_CHAIN_SIZE+LARM_CHAIN_SIZE); + qdot_LA << qdot_LOG.segment(LLEG_CHAIN_BEGIN), + qdot_LOG.segment(LARM_CHAIN_BEGIN); + velocity = (j_LOG*qdot_LA).head(3); + velocityError_LOG = velocityTarget_LOG - velocity; + + INFO("end effector position [" << ee_position_LOG(0) <<", "<first == RIGHT_ARM) || (logger->first == "Rbound") ) + { + INFO("-------- task name: "<< logger->first <<" --------"); + + INFO("task status: " << (logger->second)->taskStatus() ); + INFO("priority: " << (logger->second)->getPriority()); + INFO("activation function: " << (logger->second)->activationValue()); + + Eigen::Vector3d ee_position_LOG; + Eigen::Matrix4d h_LOG; + ((logger->second)->kinChain())->forward(&h_LOG); + ee_position_LOG = h_LOG.topRightCorner(3,1); + Eigen::Vector3d velocity; + Eigen::Vector3d velocityError_LOG; + Eigen::Vector3d velocityTarget_LOG; + velocityTarget_LOG = (logger->second)->getTargetVelocity(); + Eigen::MatrixXd j_LOG; + ((logger->second)->kinChain())->differential(&j_LOG); + Eigen::VectorXd qdot_RA (LLEG_CHAIN_SIZE+RARM_CHAIN_SIZE); + qdot_RA << qdot_LOG.segment(LLEG_CHAIN_BEGIN), + qdot_LOG.segment(RARM_CHAIN_BEGIN); + velocity = (j_LOG*qdot_RA).head(3); + velocityError_LOG = velocityTarget_LOG - velocity; + + INFO("end effector position [" << ee_position_LOG(0) <<", "<