From ace83da0311ce3dc1ebf7cfb8fa9aaf3d4311a31 Mon Sep 17 00:00:00 2001 From: Francesco Riccio <riccio.fran@gmail.com> Date: Tue, 28 Jan 2014 17:49:56 +0100 Subject: [PATCH] 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))<EPSILON*EPSILON ); +// assert( std::abs(ML_(Irn(j),sizeM))<EPSILON*EPSILON ); } /* Commute the lines in L. */ diff --git a/naoqi/modules/R2module/src/r2Module.cpp b/naoqi/modules/R2module/src/r2Module.cpp index 70a6537..cc4653e 100644 --- a/naoqi/modules/R2module/src/r2Module.cpp +++ b/naoqi/modules/R2module/src/r2Module.cpp @@ -16,7 +16,8 @@ - Simulations */ -#define INFO(x) std::cerr << "\033[22;34;1m" << "[r2module] " << x << "\033[0m" << std::endl; +//#define INFO(x) std::cerr << "\033[22;34;1m" << "[r2module] " << x << "\033[0m" << std::endl; +#define INFO(x) std::cout << x << std::endl; namespace AL { @@ -208,6 +209,39 @@ void R2Module::init() taskManager.createTask(RIGHT_LEG, Rsupporting); taskManager.createTask(LEFT_LEG, Lsupporting); +// Task* rleg1 = new Task(RLEG_TASK_DIM, RLEG_CHAIN_SIZE, 1, *theKinChainRightLeg, 0, soth::Bound::BOUND_INF); +// Task* lleg1 = new Task(LLEG_TASK_DIM, LLEG_CHAIN_SIZE, 1, *theKinChainLeftLeg, 0, soth::Bound::BOUND_INF); +// Task* rleg2 = new Task(RLEG_TASK_DIM, RLEG_CHAIN_SIZE, 1, *theKinChainRightLeg, 0, soth::Bound::BOUND_SUP); +// Task* lleg2 = new Task(LLEG_TASK_DIM, LLEG_CHAIN_SIZE, 1, *theKinChainLeftLeg, 0, soth::Bound::BOUND_SUP); +// taskManager.createTask("rleg1",rleg1); +// taskManager.createTask("lleg1", lleg1); +// taskManager.createTask("rleg2",rleg2); +// taskManager.createTask("lleg2", lleg2); + + Task* Rbound = new Task(RARM_TASK_DIM, RARM_CHAIN_SIZE+LLEG_CHAIN_SIZE, + 2, *theKinChain_LLBase + *CoM_RightArm + *theKinChainRightArm, + LLEG_CHAIN_SIZE, soth::Bound::BOUND_INF); + taskManager.createTask("Rbound", Rbound); + +// Task* Rpointing2 = new Task(RARM_TASK_DIM, RARM_CHAIN_SIZE+LLEG_CHAIN_SIZE, +// 4, *theKinChain_LLBase + *CoM_RightArm + *theKinChainRightArm, +// LLEG_CHAIN_SIZE); +// taskManager.createTask("jesus",Rpointing2); + +// Task* Lpointing1 = new Task(3, LARM_CHAIN_SIZE, 2, *theKinChainLeftArm); +// Task* Lpointing2 = new Task(3, LARM_CHAIN_SIZE, 3, *theKinChainLeftArm); +// Task* Lpointing3 = new Task(3, LARM_CHAIN_SIZE, 3, *theKinChainLeftArm); +// taskManager.createTask("LJesus",Lpointing1); +// taskManager.createTask("LMary",Lpointing2); +// taskManager.createTask("LHolySpirit",Lpointing3); + +// Task* Rpointing1 = new Task(3, RARM_CHAIN_SIZE, 2, *theKinChainRightArm); +// Task* Rpointing2 = new Task(3, RARM_CHAIN_SIZE, 3, *theKinChainRightArm); +// Task* Rpointing3 = new Task(3, RARM_CHAIN_SIZE, 3, *theKinChainRightArm); +// taskManager.createTask("RJesus",Rpointing1); +// taskManager.createTask("RMary",Rpointing2); +// taskManager.createTask("RHolySpirit",Rpointing3); + #ifdef DEBUG_MODE INFO("Initializing tasks: "); INFO("- Head task, with priority " << looking->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"<<desiredLHandPose<<std::endl; + // Release the proxy fCamProxy->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: "<<counter<<" ----------------------------"); +#endif + if(counter == 35) + taskManager.task("Rbound").activate(0.1); + +// taskManager.changePriority("Rbound", 2); + +#ifdef DEBUG_MODE + if (1/*counter == 40*/) + { +// Eigen::VectorXd desiredRLegPose = taskManager.task(RIGHT_LEG).getTargetPose(); +// Eigen::VectorXd desiredLLegPose = taskManager.task(LEFT_LEG).getTargetPose(); +// desiredRLegPose(2) += 100.0; +// desiredLLegPose(2) += 100.0; + +// taskManager.task(LEFT_LEG).setDesiredPose(desiredLLegPose, LLEG_TASK_NSTEPS); +// taskManager.task(RIGHT_LEG).setDesiredPose(desiredRLegPose, RLEG_TASK_NSTEPS); +// taskManager.changePriority("Rbound", 2); +// INFO("Changing Rbound task!"); + } +#endif #ifdef UP_DOWN_TASK if ( (taskManager.task(RIGHT_LEG).done()) && (taskManager.task(LEFT_LEG).done()) ) { - UP_DOWN *= -1; - - Eigen::VectorXd desiredRLegPose = taskManager.task(RIGHT_LEG).getTargetPose(); - Eigen::VectorXd desiredLLegPose = taskManager.task(LEFT_LEG).getTargetPose(); - desiredRLegPose(2) += UP_DOWN; - desiredLLegPose(2) += UP_DOWN; - 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); + if (updown_counter == 1) + { + UP_DOWN *= -1; + + Eigen::VectorXd desiredRLegPose = taskManager.task(RIGHT_LEG).getTargetPose(); + Eigen::VectorXd desiredLLegPose = taskManager.task(LEFT_LEG).getTargetPose(); + desiredRLegPose(2) += UP_DOWN; + desiredLLegPose(2) += UP_DOWN; + + taskManager.task(RIGHT_LEG).setDesiredPose(desiredRLegPose, RLEG_TASK_NSTEPS); + taskManager.task(LEFT_LEG).setDesiredPose(desiredLLegPose, LLEG_TASK_NSTEPS); + + if (mary_holyspirit_flag == 1) + { + taskManager.changePriority("LJesus",3); + taskManager.changePriority("LHolySpirit",2); + taskManager.changePriority("RJesus",3); + taskManager.changePriority("RHolySpirit",2); +// taskManager.task("LMary").stop(1.0); + mary_holyspirit_flag = 2; + } + else if (mary_holyspirit_flag == 2) + { + taskManager.changePriority("LJesus",2); + taskManager.changePriority("LHolySpirit",3); + taskManager.changePriority("RJesus",2); + taskManager.changePriority("RHolySpirit",3); +// taskManager.task("LMary").stop(1.0); + mary_holyspirit_flag = 1; + } + + updown_counter = 0; + } + else + ++updown_counter; } #endif // Retrieve current robot configuration Eigen::VectorXd q(JOINTS_NUM); q = getConfiguration(); + INFO("current configuration: \n" << q); #ifdef DEBUG_MODE INFO("\nCurrent joint configuration:"); diff --git a/naoqi/modules/R2module/src/task.cpp b/naoqi/modules/R2module/src/task.cpp index 6889f44..5ac6e00 100644 --- a/naoqi/modules/R2module/src/task.cpp +++ b/naoqi/modules/R2module/src/task.cpp @@ -563,8 +563,6 @@ void Task::update( const Eigen::VectorXd& _q, const Eigen::VectorXd& desiredVel, // Equality task with velocity control in the Cartesian space else if ( bounds[0].getType() == soth::Bound::BOUND_TWIN ) { - parameters.targetVelocity = K * desiredVel; - // Updating the bound vector with the task error + a feedforward term for(int i=0; i<bounds.rows(); ++i) bounds[i] = parameters.targetVelocity(i) * parameters.activationValue + @@ -623,7 +621,6 @@ void Task::update( const Eigen::VectorXd& _q, const Eigen::VectorXd& desiredVel, // Equality task with velocity control in the joint space else if ( bounds[0].getType() == soth::Bound::BOUND_TWIN ) { - parameters.targetVelocity = K * desiredVel; // Updating the bound vector with the task error + a feedforward term for(int i=0; i<bounds.rows(); ++i) { diff --git a/naoqi/modules/R2module/src/taskManager.cpp b/naoqi/modules/R2module/src/taskManager.cpp index ce77ae8..30ac05b 100644 --- a/naoqi/modules/R2module/src/taskManager.cpp +++ b/naoqi/modules/R2module/src/taskManager.cpp @@ -11,8 +11,8 @@ #include "r2Module.h" -#define INFO(x) std::cerr << "\033[22;34;1m" << "[TaskManager] " << x << "\033[0m" << std::endl; - +//#define INFO(x) std::cerr << "\033[22;34;1m" << "[TaskManager] " << x << "\033[0m" << std::endl; +#define INFO(x) std::cout << x << std::endl; using namespace AL; void R2Module::TaskManager::taskUpdate(const std::string& taskName, const Eigen::VectorXd& q) @@ -31,26 +31,47 @@ void R2Module::TaskManager::taskUpdate(const std::string& taskName, const Eigen: #endif } - else if( (taskName == LEFT_ARM) ) + else if(taskName == LEFT_ARM) { // Left arm task update - Eigen::VectorXd q_LA (LLEG_CHAIN_SIZE+LARM_CHAIN_SIZE); + Eigen::VectorXd q_LA (LLEG_CHAIN_SIZE+RARM_CHAIN_SIZE); q_LA << q.segment<LLEG_CHAIN_SIZE>(LLEG_CHAIN_BEGIN), q.segment<LARM_CHAIN_SIZE>(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_SIZE>(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_SIZE>(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_SIZE>(LLEG_CHAIN_BEGIN), q.segment<RARM_CHAIN_SIZE>(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_SIZE>(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_SIZE>(RLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(RLEG_TASK_DIM), K_RLEG ); +#else + ( taskMap[taskName] )->update( q.segment<RLEG_CHAIN_SIZE>(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_SIZE>(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_SIZE>(LLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(LLEG_TASK_DIM), K_LLEG ); - +#else + ( taskMap[taskName] )->update( q.segment<RLEG_CHAIN_SIZE>(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<std::string,Task*>::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<std::string,Task*>::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_SIZE>(LLEG_CHAIN_BEGIN), + qdot_LOG.segment<LARM_CHAIN_SIZE>(LARM_CHAIN_BEGIN); + velocity = (j_LOG*qdot_LA).head(3); + velocityError_LOG = velocityTarget_LOG - velocity; + + INFO("end effector position [" << ee_position_LOG(0) <<", "<<ee_position_LOG(1)<<", "<<ee_position_LOG(2)<<"]"); + INFO("velocity [" << velocity(0) <<", "<<velocity(1)<<", "<<velocity(2)<<"]"); + INFO("desired velocity [" << velocityTarget_LOG(0) <<", "<<velocityTarget_LOG(1)<<", "<<velocityTarget_LOG(2)<<"]"); + INFO("velocity error [" << velocityError_LOG(0) <<", "<<velocityError_LOG(1) <<", "<<velocityError_LOG(2)<<"]"); + INFO("velocity error norm: "<< velocityError_LOG.norm()); + } + if( (logger->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_SIZE>(LLEG_CHAIN_BEGIN), + qdot_LOG.segment<RARM_CHAIN_SIZE>(RARM_CHAIN_BEGIN); + velocity = (j_LOG*qdot_RA).head(3); + velocityError_LOG = velocityTarget_LOG - velocity; + + INFO("end effector position [" << ee_position_LOG(0) <<", "<<ee_position_LOG(1)<<", "<<ee_position_LOG(2)<<"]"); + INFO("velocity [" << velocity(0) <<", "<<velocity(1)<<", "<<velocity(2)<<"]"); + INFO("desired velocity [" << velocityTarget_LOG(0) <<", "<<velocityTarget_LOG(1)<<", "<<velocityTarget_LOG(2)<<"]"); + INFO("velocity error [" << velocityError_LOG(0) <<", "<<velocityError_LOG(1) <<", "<<velocityError_LOG(2)<<"]"); + INFO("velocity error norm: "<< velocityError_LOG.norm()); + } + + ++logger; + } +#endif }