From b8f062984bb2595340e517ca96a781de775f4ed5 Mon Sep 17 00:00:00 2001 From: Francesco Riccio Date: Thu, 5 Dec 2013 15:27:32 +0100 Subject: [PATCH] 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