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 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/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 f94325d..d3e31c6 100644 --- a/naoqi/modules/R2module/config/dh_head.cfg +++ b/naoqi/modules/R2module/config/dh_head.cfg @@ -1,7 +1,6 @@ [ 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 +H 0 -M_PI/2 0 q1 : HeadYaw +H 0 M_PI/2 0 q2 : HeadPitch } + diff --git a/naoqi/modules/R2module/config/dh_leftArm.cfg b/naoqi/modules/R2module/config/dh_leftArm.cfg index 5d94fde..42e84d2 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 0 0 q2 -T 105 15 0 +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 0 0 0 q3 +H -15 M_PI/2 105 q3 : LElbowYaw R 0 0 1 M_PI/2 -R 0 1 0 -M_PI/2 -H 0 0 0 q4 +H 0 0 0 q4 : LElbowRoll T 113.7 0 12.31 -R 0 1 0 M_PI/2 -H 0 0 0 q5 +R 0 0 1 -M_PI/2 +R 1 0 0 -M_PI/2 +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 3abd343..84f317e 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 : RShoulderPitch +H 0 0 0 q2 : RShoulderRoll R 0 0 1 -M_PI/2 -H 0 -M_PI/2 0 q3 -T 0 -12.31 0 -H 0 0 0 q4 -H 0 0 113.7 q5 +R 1 0 0 -M_PI/2 +H 15 M_PI/2 105 q3 : RElbowYaw +R 0 0 1 M_PI/2 +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 : 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 new file mode 100644 index 0000000..25cbfd1 --- /dev/null +++ b/naoqi/modules/R2module/include/configParams.h @@ -0,0 +1,249 @@ +#ifndef CONFIG_PARAMS +#define CONFIG_PARAMS + +#include + +//#define DEBUG_MODE +//#define TASK_DEBUG +//#define TASK_MANAGER_DEBUG +#define LOG + + +// 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/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 +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"; + + + +/** ---------------------------- 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; + +// Nao joints number +static const int JOINTS_NUM = HEAD_CHAIN_SIZE + + LARM_CHAIN_SIZE + RARM_CHAIN_SIZE + + LLEG_CHAIN_SIZE + RLEG_CHAIN_SIZE; + +// 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; + + + +/** ---------------------------- Task parameters ---------------------------- */ + +static const float ACTIVATION_STEP = 0.1; + +// 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; +static const int RARM_TASK_DIM = 3; +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 = 3; +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.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 = 30; +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 +enum TaskType { + MIRROR_TASK, + MIMIC_TASK +}; +static const TaskType ARMS_TASK = MIRROR_TASK; +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 = 80.0; +static const int CIRCLE_LAPS = 10; + +// Up-down with legs task +//#define UP_DOWN_TASK +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] +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 = 0.0; +// 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; + + +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; + + + /*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/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 0b0e0c4..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,9 @@ 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); /* * Compute the Moore-Penrose pseudo-inversion of matrix 'm' using SVD decomposition. diff --git a/naoqi/modules/R2module/include/libmath/kinChain.h b/naoqi/modules/R2module/include/libmath/kinChain.h index 684f458..b4fbdf8 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" @@ -38,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 @@ -65,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 5db8d12..1b9d481 100644 --- a/naoqi/modules/R2module/include/r2Module.h +++ b/naoqi/modules/R2module/include/r2Module.h @@ -22,9 +22,7 @@ #include #include #include - -// Soth includes -#include +#include // Libmath includes #include "libmath/kinChain.h" @@ -53,6 +51,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; @@ -69,15 +68,92 @@ class R2Module : public AL::ALModule std::vector jointID; // Container for the jointBounds Eigen::MatrixXd jointBounds; - // Joint limits primary task - TaskBase* jointLimits; + + // 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; + Rmath::KinChain* theKinChain_RLBase; + Rmath::KinChain* theKinChainLeftLeg; - // All initialized tasks - std::map taskMap; - // Stack of tasks currently active - std::set taskSet; + 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: @@ -102,10 +178,13 @@ 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 - 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 946a415..7e5d7b6 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,23 +98,154 @@ 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; + + 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, int _base, int _ee); + 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 status taskStatus(){ return parameters.taskStatus; } + 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()); + 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; + // Retrieve the current target velocity + inline const Eigen::VectorXd& getTargetVelocity() const + { + return parameters.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 + { + if ((parameters.taskStatus == inactive)) + return false; + else + 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 ); + // 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() + { + 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 ); + // 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 ); }; #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/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))* _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,13 +86,14 @@ 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 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); } @@ -100,19 +101,20 @@ 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); } + + 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) @@ -133,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/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: "< 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. */ @@ -240,15 +218,16 @@ void Rmath::KinChain::differential(int base, int ee, Eigen::MatrixXd* J, int tas // Collect all joint positions within the chain std::vector 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 @@ -285,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() ); } /* @@ -307,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; } @@ -327,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) @@ -335,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); } /* @@ -348,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/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..cc4653e 100644 --- a/naoqi/modules/R2module/src/r2Module.cpp +++ b/naoqi/modules/R2module/src/r2Module.cpp @@ -8,56 +8,25 @@ #include "r2Module.h" #include "configReader.h" +#include "configParams.h" /** TODO: - - check transformations - - task formalization + - rearraging priorities + - Simulations */ -#define INFO(x) std::cerr << "\033[22;34;1m" << "[r2module] " << x << "\033[0m" << std::endl; -//#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 +//#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 { -// 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. */ 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"); @@ -99,18 +68,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(); } /* @@ -119,20 +76,46 @@ 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, HEAD + R_ARM + L_ARM + R_LEG, - HEAD + R_ARM + L_ARM + R_LEG + L_LEG); - -#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); -#endif + // Static initialization of parameters + initialization(); + + 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 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 -------------------------------------------- */ @@ -143,6 +126,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,37 +172,83 @@ 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 - jointLimits = new TaskBase(0, Eigen::MatrixXd::Identity( JOINTS_NUM, JOINTS_NUM ), velBounds); + taskManager.setJointLimits(velBounds); #ifdef DEBUG_MODE INFO("Initializing: retrieving joint limits... "); #endif - /* ------------------------------ Other tasks: head (priority = 1), left arm (priority = 2) ------------------------------ */ + /* ------------------------------------------------- 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(3, HEAD, 1, headConfig, 0, HEAD); - // Extract joint bounds from the configuration file - ConfigReader pointingConfig( 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); - - // Push every task into the task map - taskMap.insert( std::pair ("Head task", head) ); - taskMap.insert( std::pair ("Left arm task", pointing) ); + Task* Rsupporting = new Task(RLEG_TASK_DIM, RLEG_CHAIN_SIZE, RLEG_TASK_PRIORITY, *theKinChainRightLeg); + + Task* Lsupporting = new Task(LLEG_TASK_DIM, LLEG_CHAIN_SIZE, LLEG_TASK_PRIORITY, *theKinChainLeftLeg); + + 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 + 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); + +// 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("- Joint limits, with priority " << jointLimits->getPriority()); - INFO("- Head task, with priority " << head->getPriority()); - INFO("- Left arm task, with priority " << pointing->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 } @@ -310,6 +340,7 @@ void R2Module::getCurrentFrame() */ void R2Module::vision() { +// fBallTrackerProxy->startTracker(); // Wait a while before starting acquisition qi::os::sleep(1.5); // Main loop @@ -317,7 +348,9 @@ void R2Module::vision() { qi::os::msleep(100); /* WORK IN PROGRESS */ +// INFO("ball position w.r.t. CoM: \n"<stopTracker(); } /* @@ -339,89 +372,226 @@ void R2Module::motion() #endif // Turn tasks to active - jointLimits->activate(); - taskMap["Head task"]->activate(); - taskMap["Left arm task"]->activate(); + 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); + + /****/ +// 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); + + // 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); + + /****/ +// 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, 5 ); + + if(taskManager.task(LEFT_LEG).taskStatus() != inactive) + taskManager.task(LEFT_LEG).setDesiredPose( desiredLLegPose, 5 ); +#endif + + if(taskManager.task(HEAD_TASK).taskStatus() != inactive) + 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, LARM_TASK_NSTEPS ); +#else + 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, RARM_TASK_NSTEPS ); +#else + taskManager.task(RIGHT_ARM).circularPathGenerator(desiredRHandPose, CIRCLE_Z_DEPTH, + RARM_TASK_NSTEPS, CIRCLE_RADIUS, CIRCLE_LAPS ); +#endif +#endif - // Main loop - while(true) - { - // 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); + int counter = 0; + // Main loop + while(true) + { +#ifdef LOG + ++counter; + INFO("----------------------- iteration: "< 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() ) + +#ifdef UP_DOWN_TASK + if ( (taskManager.task(RIGHT_LEG).done()) && (taskManager.task(LEFT_LEG).done()) ) { - A.push_back((*updater)->constraintMatrix()); - b.push_back((*updater)->vectorBounds()); - ++updater; + 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; } - hsolver.pushBackStages(A, b); +#endif + // Retrieve current robot configuration + Eigen::VectorXd q(JOINTS_NUM); + q = getConfiguration(); + INFO("current configuration: \n" << q); - // Configure the solver - hsolver.setDamping(0.0); - hsolver.setInitialActiveSet(); + #ifdef DEBUG_MODE + INFO("\nCurrent joint configuration:"); + for(unsigned int i = 0; i < q.size(); ++i) + INFO(jointID.at(i) << "\t" << q(i)); + #endif -#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(HEAD + R_ARM) = J_TMP_pinv * r; - INFO("Solution: [\n" << qdot << "]"); -#endif + // Solve the HQP problem with the current task set + Eigen::VectorXd qdot(JOINTS_NUM); + taskManager.exec(q, &qdot); // Update the robot configuration with the obtained solution updateConfiguration(q + qdot*TIME_STEP); - } } @@ -433,92 +603,18 @@ 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); - - return currentConfiguration; -} - -/* - * Update every active task with the current joint configuration. - */ -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); - // Left leg joints are those on the bottom part of the IDs vector - theKinChainLeftLeg->update(q.tail(L_LEG)); - // Compute kinematics - theKinChainLeftLeg->forward(&H_base); - theKinChainLeftLeg->differential(&J_base); - - /* ------------------------- 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(HEAD + R_ARM); - 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 left arm task update --------------------*/ - - // Defining the desired pose vectors in the task space - Eigen::VectorXd desiredHeadPose(3), desiredLHandPose(3); - - //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) ); - -#ifdef DEBUG_MODE - INFO("Desired head position in space: [\n" << desiredHeadPose << "]"); - INFO("Head task constraint equation: "); - INFO(std::endl << *( taskMap["Head task"] ) ); -#endif + jointId.arrayPush( jointID.at(i) ); - // Left arm task update - ( taskMap["Left arm task"] )->update( q.segment(HEAD+R_ARM), K_HEAD, desiredLHandPose, Eigen::VectorXd::Zero(3) ); + angles = fMotionProxy->getAngles( jointId, true ); -#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 - -#ifdef TEST_KINCHAIN - INFO("Desired left hand position in space: [\n" << desiredLHandPose << "]"); - r = K_HEAD * ( desiredLHandPose - H_TMP.topRightCorner(3,1) ); - INFO("New target velocity:\n[" << r << "]"); -#endif + for(int i=0; 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) ); @@ -587,5 +657,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/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 a647070..5ac6e00 100644 --- a/naoqi/modules/R2module/src/task.cpp +++ b/naoqi/modules/R2module/src/task.cpp @@ -9,10 +9,12 @@ #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 +#define POSE_ERROR_TOLERANCE 50 /* [mm] */ + /** ---------- TaskBase ---------- */ @@ -22,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() ); @@ -31,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); @@ -43,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() ); @@ -55,7 +58,7 @@ TaskBase::TaskBase(int _priority, const Eigen::MatrixXd& A, const Eigen::MatrixX for(int i=0; iupdate(q); +void Task::stop(float decayStep) +{ + if(parameters.taskStatus != inactive && parameters.taskStatus != active2inactive ) + { + parameters.activationStep = decayStep; + if(parameters.activationValue == 0.0) + parameters.taskStatus = inactive; + else + parameters.taskStatus = active2inactive; + } +} + + +Eigen::VectorXd Task::getCurrentPose() const +{ + Eigen::Matrix4d H_chain; + theKinChain->forward((&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 + // 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; +} +/* TOCOMMENT */ +const Eigen::VectorXd Task::getTargetPose() const +{ + if(parameters.positioningActive) + return parameters.path.at(parameters.path_currentStep); + else + return getCurrentPose(); +} + + +void Task::setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints) +{ // 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()); + H_chain = parameters.baseT * 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()); - // Replacing the task constraint matrix with the updated version - constraint_matrix << J_chain; + assert(dp.size() == initialPose.size()); -#ifdef DEBUG_MODE - INFO("New constraint matrix:"); - INFO(constraint_matrix); -#endif + parameters.path.clear(); + for (float i = 1.0; i <= n_controlPoints; ++i) + parameters.path.push_back(initialPose + (i/static_cast(n_controlPoints)) * (dp - initialPose)); + + parameters.path_currentStep = 0; + parameters.positioningActive = true; + if (parameters.jointControlActive) parameters.jointControlActive = false; +} - // 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 +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 = parameters.baseT * 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 - currentPose << H_chain.topRightCorner(POSITION_TASK_DIM,1), + 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 - currentPose << H_chain.col(POSITION_TASK_DIM).head(constraint_matrix.rows()); + initialPose << H_chain.col(POSITION_TASK_DIM).head(constraint_matrix.rows()); + + assert(idp.size() == initialPose.size()); + assert(dp.size() == initialPose.size()); + + parameters.path.clear(); + for (float i = 1.0; i <= n_controlPoints; ++i) + parameters.path.push_back(initialPose + (i/static_cast(n_controlPoints)) * (idp - initialPose)); + + for (float i = 1.0; i <= n_controlPoints; ++i) + parameters.path.push_back(idp + (i/static_cast(n_controlPoints)) * (dp - idp)); + + parameters.path_currentStep = 0; + parameters.positioningActive = true; + if (parameters.jointControlActive) parameters.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); + + parameters.path.clear(); + for (float i = 1.0; i <= n_controlPoints; ++i) + parameters.path.push_back(initialConf + (i/static_cast(n_controlPoints)) * (desiredConf - initialConf)); + + 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 ) +{ + + // Re-computing direct kinematics + Eigen::Matrix4d H_chain; + theKinChain->forward((&H_chain)); + H_chain = parameters.baseT * H_chain; + + 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); + 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()); + 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); + + parameters.path.push_back( ee_desiredPose_CoMframe ); + } + + 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 ) +{ + // Dimensions must be consistent + assert(constraint_matrix.rows() == desiredVel.size()); + assert(constraint_matrix.cols() == _q.size()); + + Eigen::VectorXd q(_q.size()); + q << -_q.head(base_end).reverse(), _q.tail(_q.size()-base_end); + + // Updating the task kinematic chain with the new joint values + theKinChain->update(q); + +#ifdef TASK_DEBUG + INFO("Updating task..."); + INFO("Current joint configuration: \n" << q); + INFO("Kinematic chain: \n" << (*theKinChain)); +#endif + + // 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) = 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 << (parameters.baseT.topLeftCorner(3,3) * J_chain).topRows(constraint_matrix.rows()); + } + +#ifdef TASK_DEBUG + INFO("New constraint matrix:"); + INFO(std::endl << constraint_matrix); +#endif + Eigen::VectorXd transitionVelocity(constraint_matrix.rows()); + transitionVelocity = constraint_matrix * parameters.qd_n; + + // 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 = 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 pose_error; + if(parameters.path_currentStep < parameters.path.size()) + pose_error = parameters.path.at(parameters.path_currentStep) - currentPose; + else + 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; ijointConfiguration(); + else + 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; +#define INFO(x) std::cout << x << 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+RARM_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 ); + + } +// 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 ); + // 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] )->setTargetVelocity(desiredRHandVel); + ( 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 == "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] ) ); +#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 == "Rbound") || (watcher->first == RIGHT_ARM) ) + { + 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); + +#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) <<", "<