diff --git a/naoqi/modules/R2module/config/CoM_head.cfg b/naoqi/modules/R2module/config/CoM_head.cfg new file mode 100644 index 0000000..69411ea --- /dev/null +++ b/naoqi/modules/R2module/config/CoM_head.cfg @@ -0,0 +1,4 @@ +[ T ] = +{ +T 0 0 126.5 +} diff --git a/naoqi/modules/R2module/config/CoM_leftArm.cfg b/naoqi/modules/R2module/config/CoM_leftArm.cfg new file mode 100644 index 0000000..87d79c9 --- /dev/null +++ b/naoqi/modules/R2module/config/CoM_leftArm.cfg @@ -0,0 +1,5 @@ +[ T ] = +{ +T 0 98 100 +R 1 0 0 -M_PI/2 +} diff --git a/naoqi/modules/R2module/config/CoM_leftLeg.cfg b/naoqi/modules/R2module/config/CoM_leftLeg.cfg new file mode 100644 index 0000000..9591dab --- /dev/null +++ b/naoqi/modules/R2module/config/CoM_leftLeg.cfg @@ -0,0 +1,9 @@ +[ T ] = +{ +T 0 50 -85 +R 1 0 0 M_PI/4 +R 1 0 0 M_PI +} +H 0 0 0 q1 : LHipYawPitch +R 1 0 0 -M_PI/4 +R 0 1 0 -M_PI/2 diff --git a/naoqi/modules/R2module/config/CoM_rightArm.cfg b/naoqi/modules/R2module/config/CoM_rightArm.cfg new file mode 100644 index 0000000..66633ed --- /dev/null +++ b/naoqi/modules/R2module/config/CoM_rightArm.cfg @@ -0,0 +1,5 @@ +[ T ] = +{ +T 0 -98 100 +R 1 0 0 -M_PI/2 +} diff --git a/naoqi/modules/R2module/config/CoM_rightLeg.cfg b/naoqi/modules/R2module/config/CoM_rightLeg.cfg new file mode 100644 index 0000000..17cb4cc --- /dev/null +++ b/naoqi/modules/R2module/config/CoM_rightLeg.cfg @@ -0,0 +1,9 @@ +[ T ] = +{ +T 0 -50 -85 +R 1 0 0 -M_PI/4 +} +H 0 0 0 q1 : RHipYawPitch +R 1 0 0 -M_PI/4 +R 1 0 0 -M_PI/2 +R 0 1 0 M_PI/2 diff --git a/naoqi/modules/R2module/config/dh_LAbase.cfg b/naoqi/modules/R2module/config/dh_LAbase.cfg new file mode 100644 index 0000000..a468b20 --- /dev/null +++ b/naoqi/modules/R2module/config/dh_LAbase.cfg @@ -0,0 +1,8 @@ +[ chain ] = +{ +: LWristYaw +: LElbowRoll +: LElbowYaw +: LShoulderRoll +: LShoulderPitch +} diff --git a/naoqi/modules/R2module/config/dh_LLbase.cfg b/naoqi/modules/R2module/config/dh_LLbase.cfg new file mode 100644 index 0000000..40be273 --- /dev/null +++ b/naoqi/modules/R2module/config/dh_LLbase.cfg @@ -0,0 +1,17 @@ +[ chain ] = +{ +H 0 M_PI/2 0 q1 : LAnkleRoll +H 102.9 0 0 q2 : LAnklePitch +H 100 0 0 q3 : LKneePitch +H 0 -M_PI/2 0 q4 : LHipPitch +H 0 0 0 q5 : LHipRoll +R 1 0 0 M_PI/2 +R 0 1 0 M_PI/4 +H 0 0 0 q6 : LHipYawPitch +R 0 1 0 M_PI/4 +R 0 0 1 M_PI/2 +T 0 -50 85 +} + + + diff --git a/naoqi/modules/R2module/config/dh_RAbase.cfg b/naoqi/modules/R2module/config/dh_RAbase.cfg new file mode 100644 index 0000000..e8f45ff --- /dev/null +++ b/naoqi/modules/R2module/config/dh_RAbase.cfg @@ -0,0 +1,8 @@ +[ chain ] = +{ +: RWristYaw +: RElbowRoll +: RElbowYaw +: RShoulderRoll +: RShoulderPitch +} diff --git a/naoqi/modules/R2module/config/dh_RLbase.cfg b/naoqi/modules/R2module/config/dh_RLbase.cfg new file mode 100644 index 0000000..f19436e --- /dev/null +++ b/naoqi/modules/R2module/config/dh_RLbase.cfg @@ -0,0 +1,15 @@ +[ chain ] = +{ +H 0 M_PI/2 0 q1 : RAnkleRoll +H 102.9 0 0 q2 : RAnklePitch +H 100 0 0 q3 : RKneePitch +H 0 -M_PI/2 0 q4 : RHipPitch +H 0 0 0 q5 : RHipRoll +R 1 0 0 M_PI/2 +H 0 0 0 q6 : RHipYawPitch +R 0 1 0 M_PI/4 +R 0 1 0 M_PI/4 +R 0 0 1 M_PI/2 +T 0 50 85 +} + diff --git a/naoqi/modules/R2module/config/dh_head.cfg b/naoqi/modules/R2module/config/dh_head.cfg index 7c2cc03..d3e31c6 100644 --- a/naoqi/modules/R2module/config/dh_head.cfg +++ b/naoqi/modules/R2module/config/dh_head.cfg @@ -1,8 +1,6 @@ [ chain ] = { -H 0 -M_PI/2 0 q1 -H 0 M_PI/2 0 q2 -T 53.9 0 67.9 +H 0 -M_PI/2 0 q1 : HeadYaw +H 0 M_PI/2 0 q2 : HeadPitch } -# CoM-head shift -T 0 0 126.5 + diff --git a/naoqi/modules/R2module/config/dh_leftArm.cfg b/naoqi/modules/R2module/config/dh_leftArm.cfg index 8a514cf..42e84d2 100644 --- a/naoqi/modules/R2module/config/dh_leftArm.cfg +++ b/naoqi/modules/R2module/config/dh_leftArm.cfg @@ -1,14 +1,14 @@ [ chain ] = { -H 0 M_PI/2 0 q1 -H 0 0 0 q2 +H 0 M_PI/2 0 q1 : LShoulderPitch +H 0 0 0 q2 : LShoulderRoll R 0 0 1 -M_PI/2 R 1 0 0 -M_PI/2 -H -15 M_PI/2 105 q3 +H -15 M_PI/2 105 q3 : LElbowYaw R 0 0 1 M_PI/2 -H 0 0 0 q4 +H 0 0 0 q4 : LElbowRoll T 113.7 0 12.31 R 0 0 1 -M_PI/2 R 1 0 0 -M_PI/2 -H 0 0 0 q5 +H 0 0 0 q5 : LWristYaw } diff --git a/naoqi/modules/R2module/config/dh_leftLeg.cfg b/naoqi/modules/R2module/config/dh_leftLeg.cfg index f07dd24..28a6306 100644 --- a/naoqi/modules/R2module/config/dh_leftLeg.cfg +++ b/naoqi/modules/R2module/config/dh_leftLeg.cfg @@ -1,18 +1,23 @@ [ chain ] = { +H 0 0 0 q1 : LHipYawPitch +R 1 0 0 -M_PI/4 R 0 1 0 M_PI/2 -R 0 0 1 M_PI -T 45.19 0 0 -H 0 M_PI/2 0 q1 -H 102.9 0 0 q2 -H 100 -M_PI/2 0 q3 -H 0 M_PI/2 0 q4 -H 0 0 0 q5 -R 0 1 0 -M_PI/4 -H 0 0 0 q6 -R 0 1 0 M_PI/4 + +H 0 0 0 q1 : LHipRoll +R 1 0 0 M_PI/2 +R 0 0 1 M_PI/2 +H 0 0 0 q2 : LHipPitch +T 0 100 0 +H 0 0 0 q3 : LKneePitch +T 0 102.9 0 +H 0 0 0 q4 : LAnklePitch +R 1 0 0 M_PI/2 R 0 0 1 M_PI/2 -T 0 0 -50 R 1 0 0 M_PI/2 -T 0 0 85 +H 0 0 0 q5 : LAnkleRoll +T 0 -45.19 0 +R 0 0 1 M_PI/2 } + + diff --git a/naoqi/modules/R2module/config/dh_rightArm.cfg b/naoqi/modules/R2module/config/dh_rightArm.cfg index 06d2d09..84f317e 100644 --- a/naoqi/modules/R2module/config/dh_rightArm.cfg +++ b/naoqi/modules/R2module/config/dh_rightArm.cfg @@ -1,14 +1,14 @@ [ chain ] = { -H 0 M_PI/2 0 q1 -H 0 0 0 q2 +H 0 M_PI/2 0 q1 : RShoulderPitch +H 0 0 0 q2 : RShoulderRoll R 0 0 1 -M_PI/2 R 1 0 0 -M_PI/2 -H 15 M_PI/2 105 q3 +H 15 M_PI/2 105 q3 : RElbowYaw R 0 0 1 M_PI/2 -H 0 0 0 q4 +H 0 0 0 q4 : RElbowRoll T 113.7 0 12.31 R 0 0 1 -M_PI/2 R 1 0 0 -M_PI/2 -H 0 0 0 q5 +H 0 0 0 q5 : RWristYaw } diff --git a/naoqi/modules/R2module/config/dh_rightLeg.cfg b/naoqi/modules/R2module/config/dh_rightLeg.cfg index c5d8a7b..8d3891c 100644 --- a/naoqi/modules/R2module/config/dh_rightLeg.cfg +++ b/naoqi/modules/R2module/config/dh_rightLeg.cfg @@ -1,14 +1,23 @@ [ chain ] = { -T 0 -50 -85 +H 0 0 0 q1 : RHipYawPitch R 1 0 0 -M_PI/4 -H 0 0 0 q1 -R 1 0 0 -M_PI/4 -H 0 -M_PI/2 0 q2 -H -100 M_PI/2 0 q3 -H -102.9 0 0 q4 -H 0 -M_PI/2 0 q5 -H -45.19 0 0 q6 -R 0 0 1 -M_PI -R 0 1 0 -M_PI/2 +R 1 0 0 -M_PI/2 +R 0 1 0 M_PI/2 + +H 0 0 0 q1 : RHipRoll +R 1 0 0 M_PI/2 +R 0 0 1 M_PI/2 +H 0 0 0 q2 : RHipPitch +T 0 100 0 +H 0 0 0 q3 : RKneePitch +T 0 102.9 0 +H 0 0 0 q4 : RAnklePitch +R 1 0 0 M_PI/2 +R 0 0 1 M_PI/2 +R 1 0 0 M_PI/2 +H 0 0 0 q5 : RAnkleRoll +T 0 -45.19 0 +R 0 0 1 M_PI/2 } + diff --git a/naoqi/modules/R2module/config/joints_params.cfg b/naoqi/modules/R2module/config/joints_params.cfg index 0e982b9..37df5cf 100644 --- a/naoqi/modules/R2module/config/joints_params.cfg +++ b/naoqi/modules/R2module/config/joints_params.cfg @@ -25,3 +25,4 @@ 0.0 -1.189516 0.922747 LAnklePitch 0.0 -0.397880 0.769001 LAnkleRoll } + diff --git a/naoqi/modules/R2module/include/configParams.h b/naoqi/modules/R2module/include/configParams.h index ee11c96..50afb37 100644 --- a/naoqi/modules/R2module/include/configParams.h +++ b/naoqi/modules/R2module/include/configParams.h @@ -3,67 +3,124 @@ #include -// CIRCLE:: n°circles, z_shift, radius +#define DEBUG_MODE -// Nao joints number -static const int JOINTS_NUM = 24; // Discrete integration time step static const double TIME_STEP = 1.0; + +// Nao numerical values +#define FOOT_HEIGHT 45.19 + +#define HIP_HEIGHT 202.9 +#define HIP_PELVIS_SHIFT 50.0 +#define HIP_TORSO 85.0 + +#define TORSO_NECK 126.5 +#define TORSO_SHOULDER_Z 100.0 +#define TORSO_SHOULDER_Y 98.0 + +#define ARM_LENGTH 218.0 +#define WRIST_SHIFT_Z 12.31 +#define ELBOW_SHIFT_Y 15.0 + + +/** ---------------------------- Paths ---------------------------- */ + + // Config (.cfg) files directory and paths //static const std::string CONFIG_PATH = "/home/claudio/Naoqi/NaoModules/r2module/config/"; static const std::string CONFIG_PATH = "/home/francesco/naoqi/modules/R2module/config/"; +// Joint parameters static const std::string JOINT_BOUNDS_CFG = "joints_params.cfg"; +// Base kinematic chains +static const std::string LEFT_LEG_BASE_CFG = "dh_LLbase.cfg"; +static const std::string RIGHT_LEG_BASE_CFG = "dh_RLbase.cfg"; +// Kinematic chains static const std::string LEFT_LEG_CFG = "dh_leftLeg.cfg"; static const std::string LEFT_ARM_CFG = "dh_leftArm.cfg"; static const std::string RIGHT_LEG_CFG = "dh_rightLeg.cfg"; static const std::string RIGHT_ARM_CFG = "dh_rightArm.cfg"; static const std::string HEAD_CFG = "dh_head.cfg"; +// Fixed transformations +static const std::string COM_HEAD_CFG = "CoM_head.cfg"; +static const std::string COM_RIGHT_ARM_CFG = "CoM_rightArm.cfg"; +static const std::string COM_LEFT_ARM_CFG = "CoM_leftArm.cfg"; +static const std::string COM_RIGHT_LEG_CFG = "CoM_rightLeg.cfg"; +static const std::string COM_LEFT_LEG_CFG = "CoM_leftLeg.cfg"; -/** ---------------------- Head task parameters ---------------------- */ +/** ---------------------------- Joint parameters ---------------------------- */ + + +// Head static const int HEAD_CHAIN_BEGIN = 0; static const int HEAD_CHAIN_SIZE = 2; static const int HEAD_CHAIN_END = HEAD_CHAIN_BEGIN + HEAD_CHAIN_SIZE; +// Right arm +static const int RARM_CHAIN_BEGIN = HEAD_CHAIN_END; +static const int RARM_CHAIN_SIZE = 5; +static const int RARM_CHAIN_END = RARM_CHAIN_BEGIN + RARM_CHAIN_SIZE; +// Left arm +static const int LARM_CHAIN_BEGIN = RARM_CHAIN_END; +static const int LARM_CHAIN_SIZE = 5; +static const int LARM_CHAIN_END = LARM_CHAIN_BEGIN + LARM_CHAIN_SIZE; +// Right leg +static const int RLEG_CHAIN_BEGIN = LARM_CHAIN_END; +static const int RLEG_CHAIN_SIZE = 6; +static const int RLEG_CHAIN_END = RLEG_CHAIN_BEGIN + RLEG_CHAIN_SIZE; +// Left leg +static const int LLEG_CHAIN_BEGIN = RLEG_CHAIN_END; +static const int LLEG_CHAIN_SIZE = 6; +static const int LLEG_CHAIN_END = LLEG_CHAIN_BEGIN + LLEG_CHAIN_SIZE; -static const int HEAD_TASK_DIM = 3; -static const int HEAD_TASK_PRIORITY = 3; -static const int K_HEAD = 1; -static const int HEAD_TASK_NSTEPS = 1; +// Nao joints number +static const int JOINTS_NUM = HEAD_CHAIN_SIZE + + LARM_CHAIN_SIZE + RARM_CHAIN_SIZE + + LLEG_CHAIN_SIZE + RLEG_CHAIN_SIZE; -static const double HEAD_DESIRED_X = 53.9; -static const double HEAD_DESIRED_Y = 0.0; -static const double HEAD_DESIRED_Z = 194.4; -static const double HEAD_DESIRED_ROLL = 0.0; -static const double HEAD_DESIRED_PITCH = 0.0; -static const double HEAD_DESIRED_YAW = 0.0; +// Fixed transformation base-ankle frame +static const double BASE_ANKLE_X = 0.0; +static const double BASE_ANKLE_Y = 0.0; +static const double BASE_ANKLE_Z = FOOT_HEIGHT; +static Eigen::Matrix4d base_ankle; -/** -------------------- Right arm task parameters -------------------- */ +/** ---------------------------- Task parameters ---------------------------- */ -static const int RARM_CHAIN_BEGIN = HEAD_CHAIN_END; -static const int RARM_CHAIN_SIZE = 5; -static const int RARM_CHAIN_END = RARM_CHAIN_BEGIN + RARM_CHAIN_SIZE; -static const double COM_RARM_SHIFT_X = 0.0; -static const double COM_RARM_SHIFT_Y = -98.0; -static const double COM_RARM_SHIFT_Z = 100.0; +// Task dimensions +static const int HEAD_TASK_DIM = 6; static const int RARM_TASK_DIM = 3; -static const int RARM_TASK_PRIORITY = 1; -static const int K_RARM = 1; +static const int LARM_TASK_DIM = 3; +static const int RLEG_TASK_DIM = 6; +static const int LLEG_TASK_DIM = 6; + +// Task priorities +static const int HEAD_TASK_PRIORITY = 5; +static const int RARM_TASK_PRIORITY = 3; +static const int LARM_TASK_PRIORITY = 2; +static const int RLEG_TASK_PRIORITY = 1; +static const int LLEG_TASK_PRIORITY = 1; + +// Gains +static const double K_HEAD = 1.0; +static const double K_RARM = 1.0; +static const double K_LARM = 1.0; +static const double K_RLEG = 0.8; +static const double K_LLEG = 0.8; + +// Control points number +static const int HEAD_TASK_NSTEPS = 1; static const int RARM_TASK_NSTEPS = 30; - -static const double RARM_DESIRED_X = 250.0; -static const double RARM_DESIRED_Y = -113.0; -static const double RARM_DESIRED_Z = 120.0; -static const double RARM_DESIRED_ROLL = 0.0; -static const double RARM_DESIRED_PITCH = 0.0; -static const double RARM_DESIRED_YAW = 0.0; +static const int LARM_TASK_NSTEPS = 30; +static const int RLEG_TASK_NSTEPS = 30; +static const int LLEG_TASK_NSTEPS = 30; #define RARM_LARM_JOINT_TASK @@ -71,58 +128,69 @@ enum TaskType { MIRROR_TASK, MIMIC_TASK }; -static const TaskType ARMS_TASK = MIMIC_TASK; -static const double MINIMUM_HANDS_DISTANCE = 50.0; - -/** -------------------- Left arm task parameters -------------------- */ - - -static const int LARM_CHAIN_BEGIN = RARM_CHAIN_END; -static const int LARM_CHAIN_SIZE = 5; -static const int LARM_CHAIN_END = LARM_CHAIN_BEGIN + LARM_CHAIN_SIZE; +static const TaskType ARMS_TASK = MIRROR_TASK; +static const double MINIMUM_HANDS_DISTANCE = 350.0; -static const double COM_LARM_SHIFT_X = 0.0; -static const double COM_LARM_SHIFT_Y = 98.0; -static const double COM_LARM_SHIFT_Z = 100.0; +// Circle trajectory task parameters +//#define LARM_CIRCLE_TASK +//#define RARM_CIRCLE_TASK +static const double CIRCLE_Z_DEPTH = -70.0; // wrt ee-frame +static const double CIRCLE_RADIUS = 70.0; +static const int CIRCLE_LAPS = 10; -static const int LARM_TASK_DIM = 3; -static const int LARM_TASK_PRIORITY = 1; -static const int K_LARM = 1; -static const int LARM_TASK_NSTEPS = 30; - -static const double LARM_DESIRED_X = 0.0; -static const double LARM_DESIRED_Y = 250 + 113.0; -static const double LARM_DESIRED_Z = 120.0; -static const double LARM_DESIRED_ROLL = 0.0; -static const double LARM_DESIRED_PITCH = 0.0; -static const double LARM_DESIRED_YAW = M_PI/2; +// Up-down with legs task +//#define UP_DOWN_TASK +static int UP_DOWN = 80.0; -/** -------------------- Right leg task parameters -------------------- */ - -static const int RLEG_CHAIN_BEGIN = LARM_CHAIN_END; -static const int RLEG_CHAIN_SIZE = 6; -static const int RLEG_CHAIN_END = RLEG_CHAIN_BEGIN + RLEG_CHAIN_SIZE; +/** ------------------------------- Desired poses ------------------------------- */ -/** -------------------- Left leg task parameters -------------------- */ - -static const int LLEG_CHAIN_BEGIN = RLEG_CHAIN_END; -static const int LLEG_CHAIN_SIZE = 6; -static const int LLEG_CHAIN_END = LLEG_CHAIN_BEGIN + LLEG_CHAIN_SIZE; - -static const int LLEG_TASK_DIM = 3; - - -#ifdef TEST_KINCHAIN -static Rmath::KinChain* theKinChain_TMP; -static Eigen::Matrix4d H_TMP; -static Eigen::MatrixXd J_TMP; -static Eigen::VectorXd q_TMP(LARM_CHAIN_SIZE); -static Eigen::Vector3d r; +//Head +static const double HEAD_DESIRED_X = 0.0; // [mm] +static const double HEAD_DESIRED_Y = 0.0; +static const double HEAD_DESIRED_Z = FOOT_HEIGHT+HIP_HEIGHT+HIP_TORSO+TORSO_NECK-9.9; +static const double HEAD_DESIRED_ROLL = 0.0; +static const double HEAD_DESIRED_PITCH = 0.0; +static const double HEAD_DESIRED_YAW = 0.0; +//Right arm +static const double RARM_DESIRED_X = ARM_LENGTH; +static const double RARM_DESIRED_Y = -HIP_PELVIS_SHIFT-TORSO_SHOULDER_Y-ELBOW_SHIFT_Y; +static const double RARM_DESIRED_Z = FOOT_HEIGHT+HIP_HEIGHT+HIP_TORSO+TORSO_SHOULDER_Z+WRIST_SHIFT_Z-9.9; +static const double RARM_DESIRED_ROLL = 0.0; +static const double RARM_DESIRED_PITCH = 0.0; +static const double RARM_DESIRED_YAW = 0.0; +// Left arm +static const double LARM_DESIRED_X = ARM_LENGTH; +static const double LARM_DESIRED_Y = -HIP_PELVIS_SHIFT+TORSO_SHOULDER_Y+ELBOW_SHIFT_Y; +static const double LARM_DESIRED_Z = FOOT_HEIGHT+HIP_HEIGHT+HIP_TORSO+TORSO_SHOULDER_Z+WRIST_SHIFT_Z-9.9; +static const double LARM_DESIRED_ROLL = 0.0; +static const double LARM_DESIRED_PITCH = 0.0; +static const double LARM_DESIRED_YAW = M_PI_2; +// Right leg +static const double RLEG_DESIRED_X = 20.0; +static const double RLEG_DESIRED_Y = -HIP_PELVIS_SHIFT; +#ifndef UP_DOWN_TASK +static const double RLEG_DESIRED_Z = -HIP_HEIGHT-FOOT_HEIGHT-HIP_TORSO+9.9; +#else +static const double RLEG_DESIRED_Z = -HIP_HEIGHT-FOOT_HEIGHT-HIP_TORSO+UP_DOWN+9.9; +#endif +static const double RLEG_DESIRED_ROLL = 0.0; +static const double RLEG_DESIRED_PITCH = 0.0; +static const double RLEG_DESIRED_YAW = 0.0; +// Left leg +static const double LLEG_DESIRED_X = 20.0; +static const double LLEG_DESIRED_Y = HIP_PELVIS_SHIFT; +#ifndef UP_DOWN_TASK +static const double LLEG_DESIRED_Z = -HIP_HEIGHT-FOOT_HEIGHT-HIP_TORSO+9.9; +#else +static const double LLEG_DESIRED_Z = -HIP_HEIGHT-FOOT_HEIGHT-HIP_TORSO+UP_DOWN+9.9; #endif +static const double LLEG_DESIRED_ROLL = 0.0; +static const double LLEG_DESIRED_PITCH = 0.0; +static const double LLEG_DESIRED_YAW = 0.0; #endif diff --git a/naoqi/modules/R2module/include/configReader.h b/naoqi/modules/R2module/include/configReader.h index 3f51f88..31a789c 100644 --- a/naoqi/modules/R2module/include/configReader.h +++ b/naoqi/modules/R2module/include/configReader.h @@ -29,9 +29,9 @@ static double to_double(std::string s){ class ConfigReader{ private: std::vector transformations; - std::vector minBounds; - std::vector maxBounds; - std::vector zero_pose; + std::vector kinChainJointIDs; + + std::map > jointParams; std::vector jointID; void dhReader(std::string dh_file_path); @@ -39,11 +39,15 @@ class ConfigReader{ double valueReader(std::string _val); public: - ConfigReader(std::string dh_file_path, std::string params_file_path = ""); + ConfigReader(std::string params_file_path); + + void setKinChain(std::string dh_file_path); + + void storeJointsID(std::vector* _jointID); - void storeJointsID(std::vector *_jointID); + void kinChainJointsID(std::vector* _kcJointID); - void extractJointParams(std::vector* _min, std::vector* _max, std::vector* _zero_pose, int _i, int _j); + std::vector getJointParams(std::string _id); soth::VectorBound extractJointBounds(); void extractJointBounds(Eigen::MatrixXd* bounds); diff --git a/naoqi/modules/R2module/include/libmath/Rutils.h b/naoqi/modules/R2module/include/libmath/Rutils.h index e63821b..399cdf3 100644 --- a/naoqi/modules/R2module/include/libmath/Rutils.h +++ b/naoqi/modules/R2module/include/libmath/Rutils.h @@ -97,7 +97,7 @@ void homogenize(const Eigen::Matrix3d& m, const Eigen::Vector3d& v, Eigen::Matri * The result is stored in 'mx'. */ void Hx(double a, const Eigen::Vector3d& shift, Eigen::Matrix4d* mx); -Eigen::Matrix4d homX(double a, const Eigen::Vector3d& shift); +Eigen::Matrix4d homX(double a, const Eigen::Vector3d& shift = Eigen::Vector3d::Zero()); /* * Compute the 4x4 homogeneous transformation composed of: @@ -106,7 +106,7 @@ Eigen::Matrix4d homX(double a, const Eigen::Vector3d& shift); * The result is stored in 'mx'. */ void Hy(double a, const Eigen::Vector3d& shift, Eigen::Matrix4d* my); -Eigen::Matrix4d homY(double a, const Eigen::Vector3d& shift); +Eigen::Matrix4d homY(double a, const Eigen::Vector3d& shift = Eigen::Vector3d::Zero()); /* * Compute the 4x4 homogeneous transformation composed of: @@ -115,7 +115,7 @@ Eigen::Matrix4d homY(double a, const Eigen::Vector3d& shift); * The result is stored in 'mx'. */ void Hz(double a, const Eigen::Vector3d& shift, Eigen::Matrix4d* mz); -Eigen::Matrix4d homZ(double a, const Eigen::Vector3d& shift); +Eigen::Matrix4d homZ(double a, const Eigen::Vector3d& shift = Eigen::Vector3d::Zero()); Eigen::Matrix4d hTranslation(const Eigen::Vector3d& shift); diff --git a/naoqi/modules/R2module/include/libmath/kinChain.h b/naoqi/modules/R2module/include/libmath/kinChain.h index 209d635..b4fbdf8 100644 --- a/naoqi/modules/R2module/include/libmath/kinChain.h +++ b/naoqi/modules/R2module/include/libmath/kinChain.h @@ -39,23 +39,26 @@ class KinChain class Joint { public: + // Identifiers + std::string id; + int col; double value; double max; double min; - Joint(double _value = 0.0, double _max = Eigen::Infinity, double _min = -Eigen::Infinity): - value(_value), max(_max), min(_min) {} + Joint(std::string _id, int _col, double _value = 0.0, double _max = Eigen::Infinity, double _min = -Eigen::Infinity): + id(_id), col(_col), value(_value), max(_max), min(_min){} }; // Indices of the robot joints within the chain std::map joints; - public: // Constructors - KinChain(ConfigReader& theConfigReader, int _i, int _j); - KinChain(std::vector _transf); + KinChain(ConfigReader& theConfigReader); +// KinChain(ConfigReader& theConfigReader, int _i, int _j); + KinChain(std::vector _transf, std::map _joints); // Copy constructors KinChain(const Rmath::KinChain& kc); // Destructors @@ -66,6 +69,8 @@ class KinChain Eigen::VectorXd jointConfiguration(); // Getter for the joint limits const soth::VectorBound jointLimits(); + // Getter for joints names and indices + void getJointsIDs(std::map* jointsIDs) const; // Joint configuration update void update(int first, int last, Eigen::VectorXd &q); diff --git a/naoqi/modules/R2module/include/r2Module.h b/naoqi/modules/R2module/include/r2Module.h index eab505c..1c89ae1 100644 --- a/naoqi/modules/R2module/include/r2Module.h +++ b/naoqi/modules/R2module/include/r2Module.h @@ -74,14 +74,26 @@ class R2Module : public AL::ALModule // Joint limits primary task TaskBase* jointLimits; - // Common base kinematic chain (left foot to body center) - Rmath::KinChain* theKinChainLeftLeg; // All initialized tasks std::map taskMap; // Stack of tasks currently active std::set taskSet; - // Tasks respective positions within the whole kinematic chain - std::map taskLoc; + + // Common base kinematic chain (left foot to body center) + Rmath::KinChain* theKinChain_LLBase; + Rmath::KinChain* theKinChain_RLBase; + + Rmath::KinChain* theKinChainLeftLeg; + Rmath::KinChain* theKinChainRightLeg; + Rmath::KinChain* theKinChainLeftArm; + Rmath::KinChain* theKinChainRightArm; + Rmath::KinChain* theKinChainHead; + + Rmath::KinChain* CoM_LeftLeg; + Rmath::KinChain* CoM_RightLeg; + Rmath::KinChain* CoM_LeftArm; + Rmath::KinChain* CoM_RightArm; + Rmath::KinChain* CoM_Head; public: diff --git a/naoqi/modules/R2module/include/task.h b/naoqi/modules/R2module/include/task.h index 79be1e3..9c92c5d 100644 --- a/naoqi/modules/R2module/include/task.h +++ b/naoqi/modules/R2module/include/task.h @@ -107,6 +107,7 @@ class Task : public TaskBase // The task kinematic chain Rmath::KinChain* theKinChain; + int base_end; // Toggle positioning/velocity task bool positioningActive; @@ -117,18 +118,43 @@ class Task : public TaskBase std::vector path; // Current progress in the path int path_currentStep; + // Current target velocity + Eigen::VectorXd targetVelocity; + public: // Constructor - Task(int m, int n, int _priority, ConfigReader theConfigReader, int _base, int _ee); + Task(int m, int n, int _priority, ConfigReader theConfigReader); + Task(int m, int n, int _priority, const Rmath::KinChain& _kc, int _base = 0); // Destructor ~Task(); +// inline Rmath::KinChain* kinChain(){ return theKinChain; } + // Retrieve the current end-effector pose Eigen::VectorXd getCurrentPose(const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity()) const; + // Retrieve the current target velocity + inline const Eigen::VectorXd& getTargetVelocity() const + { + return targetVelocity; + } + // Retrieve the current target pose + const Eigen::VectorXd getTargetPose() const; + // Retrieve the kinchain joints information + void getJointsIDs(std::map* jointsIDs) const + { + theKinChain->getJointsIDs(jointsIDs); + } + // Check if the task is done + inline bool done() const { + return (path_currentStep == path.size()-1); + } + // Set a desired pose in the task space void setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints = 1, const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity()); + void setDesiredPose(const Eigen::VectorXd& idp, const Eigen::VectorXd& dp, int n_controlPoints = 1, + const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity()); // Set a desired configuration in the joint space void setDesiredConfiguration(const Eigen::VectorXd& desiredConf, int n_controlPoints); // Reset the current target pose (either in joint or task space) @@ -143,7 +169,7 @@ class Task : public TaskBase const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity() ); // Update function - void update(const Eigen::VectorXd& q, const Eigen::VectorXd& desiredVel, + void update(const Eigen::VectorXd& _q, const Eigen::VectorXd& desiredVel, double K = 1.0, const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity()); }; diff --git a/naoqi/modules/R2module/src/configReader.cpp b/naoqi/modules/R2module/src/configReader.cpp index c0ff5a2..1327135 100644 --- a/naoqi/modules/R2module/src/configReader.cpp +++ b/naoqi/modules/R2module/src/configReader.cpp @@ -10,46 +10,38 @@ #include "configReader.h" - -ConfigReader::ConfigReader(std::string dh_file_path, std::string params_file_path) +ConfigReader::ConfigReader(std::string params_file_path) { - dhReader(dh_file_path); - if(params_file_path != ""){ - paramsReader(params_file_path); - } + paramsReader(params_file_path); } -void ConfigReader::extractJointParams(std::vector* _min, std::vector* _max, std::vector* _zero_pose, int _i, int _j) -{ - _min->clear(); - _max->clear(); - _zero_pose->clear(); + void ConfigReader::setKinChain(std::string dh_file_path) + { + transformations.clear(); + kinChainJointIDs.clear(); + dhReader(dh_file_path); + } - for(int i=_i; i<_j; ++i) - { - _min->push_back( minBounds.at(i) ); - _max->push_back( maxBounds.at(i) ); - _zero_pose->push_back( zero_pose.at(i) ); - } +std::vector ConfigReader::getJointParams(std::string _id) +{ + assert( jointParams.find(_id) != jointParams.end() ); + return jointParams[_id]; } soth::VectorBound ConfigReader::extractJointBounds() { - soth::VectorBound bounds(5);//minBounds.size()); -// for(int i=0; irow(i) << minBounds.at(i), maxBounds.at(i); - } + for(int index=0; indexrow(index) << jointParams[jointID.at(index)].at(2), + jointParams[jointID.at(index)].at(1); } void ConfigReader::storeJointsID(std::vector* _jointID) @@ -57,6 +49,11 @@ void ConfigReader::storeJointsID(std::vector* _jointID) *_jointID = jointID; } +void ConfigReader::kinChainJointsID(std::vector* _kcJointID) +{ + *_kcJointID = kinChainJointIDs; +} + void ConfigReader::dhReader(std::string dh_file_path) { std::ifstream cfg_dh; @@ -67,6 +64,8 @@ void ConfigReader::dhReader(std::string dh_file_path) return; } + kinChainJointIDs.clear(); + std::string line; while(cfg_dh.good()) { @@ -78,6 +77,7 @@ void ConfigReader::dhReader(std::string dh_file_path) std::back_inserter >(tokens)); if(tokens.empty()) continue; + if(tokens.at(0) == "#") continue; // Canonical DH transform case if (tokens.at(0) == "H") @@ -86,6 +86,7 @@ void ConfigReader::dhReader(std::string dh_file_path) valueReader(tokens.at(2)), to_double(tokens.at(3)), 0.0 ); + kinChainJointIDs.push_back(tokens.at(6)); transformations.push_back(dh_transform); } // Fixed translation case @@ -100,7 +101,7 @@ void ConfigReader::dhReader(std::string dh_file_path) else if (tokens.at(0) == "R") { Rmath::Transform* rot = new Rmath::Rotation( to_double(tokens.at(1)), - to_double(tokens.at(2)) , + to_double(tokens.at(2)), to_double(tokens.at(3)), valueReader(tokens.at(4)) ); transformations.push_back(rot); @@ -108,12 +109,12 @@ void ConfigReader::dhReader(std::string dh_file_path) if (tokens.at(0) == "}") break; } + cfg_dh.close(); } void ConfigReader::paramsReader(std::string params_file_path) { - std::fstream cfg_params; cfg_params.open(params_file_path.c_str(), std::fstream::in); if(cfg_params == NULL) @@ -134,12 +135,19 @@ void ConfigReader::paramsReader(std::string params_file_path) if(tokens.empty()) continue; if( tokens.at(0) == "[" ) continue; - if( tokens.at(0) == "{" || tokens.at(0) == "}" ) continue; + if( tokens.at(0) == "#" ) continue; + if( tokens.at(0) == "{" ) continue; + + if( tokens.at(0) == "}" ) break; + + std::vector params; + params.push_back(valueReader(tokens.at(0))); + params.push_back(to_double(tokens.at(2))); + params.push_back(to_double(tokens.at(1))); + + jointParams.insert(std::pair >(tokens.at(3), params) ); - zero_pose.push_back( valueReader(tokens.at(0)) ); - minBounds.push_back( to_double(tokens.at(1)) ); - maxBounds.push_back( to_double(tokens.at(2)) ); - jointID.push_back( tokens.at(3) ); + jointID.push_back(tokens.at(3)); } cfg_params.close(); } diff --git a/naoqi/modules/R2module/src/libmath/kinChain.cpp b/naoqi/modules/R2module/src/libmath/kinChain.cpp index 45f9062..c51f673 100644 --- a/naoqi/modules/R2module/src/libmath/kinChain.cpp +++ b/naoqi/modules/R2module/src/libmath/kinChain.cpp @@ -13,79 +13,46 @@ #define INFO(x) std::cout << "[R2 module::KinChain] " << x << std::endl; - -/* - * Class constructor using a ConfigReader class object. - * The chain of transformation is extracted from the .cfg file using indinces _i, _j and then - * the private containers are initialized. - */ -Rmath::KinChain::KinChain(ConfigReader& theConfigReader, int _i, int _j) +Rmath::KinChain::KinChain( ConfigReader& theConfigReader ) { // Extract the chain of transformations transformations = theConfigReader.Transformations(); // Extract from the general .cfg file all the information about joints - std::vector jointBounds_max, jointBounds_min, jointZeroPose; - theConfigReader.extractJointParams(&jointBounds_min, &jointBounds_max, &jointZeroPose, _i, _j); - // Check for dimensions consistency - assert(jointBounds_max.size() == jointBounds_min.size()); - assert(jointBounds_max.size() <= transformations.size()); - assert(jointZeroPose.size() >= 0); + std::vector kcJointIds; + theConfigReader.kinChainJointsID(&kcJointIds); - // Detect the actual joints across the kinematic chain and fill the joints container int joint_count = 0; for(unsigned int i = 0; i < transformations.size(); ++i) { - // A joints is identified by a DH-transform - if ( transformations.at(i)->getType() == Rmath::dh_transform ) + if( transformations.at(i)->getType() == Rmath::dh_transform ) { - // Look for data about the current joint - if(jointBounds_min.size() != 0 && jointBounds_max.size() != 0 && jointZeroPose.size() != 0) - { - // Set the zero-value of the joint (if present) - if(jointZeroPose.at(joint_count) != 0.0) - transformations.at(i)->update(jointZeroPose.at(joint_count)); - // Generate a Joint class object and push it in the container - Rmath::KinChain::Joint current_q(jointZeroPose.at(joint_count), - jointBounds_max.at(joint_count), - jointBounds_min.at(joint_count)); - joints[i] = current_q; - } - // Create a joint with no mechanical bounds and initial value = 0.0 otherwise - else - { - Rmath::KinChain::Joint current_q (0.0); - joints[i] = current_q; - } + std::vector jointParams; + jointParams = theConfigReader.getJointParams( kcJointIds.at(joint_count) ); + + Rmath::KinChain::Joint current_q( kcJointIds.at(joint_count), + joint_count, + jointParams.at(0), + jointParams.at(1), + jointParams.at(2) ); + + joints.insert( std::pair (i, current_q) ); + ++joint_count; } } + } /* * Class constructor overload using a vector of Rmath::Transform. */ -Rmath::KinChain::KinChain(std::vector _transf): transformations(_transf) -{ - // Detect the actual joints across the kinematic chain and fill the joints container - int joint_count = 0; - for(unsigned int i = 0; i < transformations.size(); ++i) - { - // A joints is identified by a DH-transform - if ( transformations.at(i)->getType() == Rmath::dh_transform ) - { - // Create a joint with no mechanical bounds and initial value = 0.0 - Rmath::KinChain::Joint current_q; - joints[i] = current_q; - - ++joint_count; - } - } - -} +Rmath::KinChain::KinChain(std::vector _transf, std::map _joints): + transformations(_transf), joints(_joints) +{} /* - * Class copy constructor (NOTE: shallow copy!). + * Class copy constructor (NOTE: deep copy!). */ Rmath::KinChain::KinChain(const Rmath::KinChain& kc) { @@ -121,6 +88,18 @@ const soth::VectorBound Rmath::KinChain::jointLimits() return bounds; } +/* TOCOMMENT */ +void Rmath::KinChain::getJointsIDs(std::map* jointsIDs) const +{ + assert(jointsIDs != NULL); + std::map::const_iterator i = joints.begin(); + while(i !=joints.end()) + { + jointsIDs->insert( std::pair( (i->second).id, (i->second).col ) ); + ++i; + } +} + /* * Get the current joint configuration (or any subset of it). */ @@ -182,7 +161,7 @@ void Rmath::KinChain::update(int first, int last, Eigen::VectorXd& q) if ((j >= first) && (j <= last) ) { // Update using the Transform base class method - joint_updater->second = q(i); + (joint_updater->second).value = q(i); transformations.at(joint_updater->first)->update(q(i)); ++i; } @@ -201,14 +180,13 @@ void Rmath::KinChain::update(const Eigen::VectorXd &q) while (joint_updater != joints.end()) { // Update using the Transform base class method - joint_updater->second = q(i); + (joint_updater->second).value = q(i); transformations.at(joint_updater->first)->update(q(i)); ++joint_updater; ++i; } } - /* * Compute the forward kinematics of the chain (or any subchain of it) as a homogeneous transformation matrix. */ @@ -286,17 +264,19 @@ Rmath::KinChain& Rmath::KinChain::operator=(const Rmath::KinChain& kc) std::map::const_iterator joint_iter = kc.joints.begin(); while (joint_iter != kc.joints.end()) { - Rmath::KinChain::Joint current_q((joint_iter->second).value, - (joint_iter->second).max, - (joint_iter->second).min); + Rmath::KinChain::Joint current_q( (joint_iter->second).id, + (joint_iter->second).col, + (joint_iter->second).value, + (joint_iter->second).max, + (joint_iter->second).min ); - joints[joint_iter->first] = current_q; + joints.insert( std::pair (joint_iter->first, current_q) ); ++joint_iter; } - // Re-fill the vector of Transform* using the input KinChain object (NOTE: shallow copy!) + // Re-fill the vector of Transform* using the input KinChain object (NOTE: deep copy!) for(int i=0; iclone() ); } /* @@ -308,11 +288,13 @@ Rmath::KinChain& Rmath::KinChain::operator+=(const Rmath::KinChain& kc) std::map::const_iterator joint_iter = kc.joints.begin(); while (joint_iter != kc.joints.end()) { - Rmath::KinChain::Joint current_q((joint_iter->second).value, - (joint_iter->second).max, - (joint_iter->second).min); + Rmath::KinChain::Joint current_q( (joint_iter->second).id, + (joint_iter->second).col, + (joint_iter->second).value, + (joint_iter->second).max, + (joint_iter->second).min); // Indices are shifted by the size of the precedent chain - joints[ transformations.size() + joint_iter->first] = current_q; + joints.insert( std::pair (transformations.size() + joint_iter->first, current_q) ); ++joint_iter; } @@ -328,6 +310,35 @@ Rmath::KinChain& Rmath::KinChain::operator+=(const Rmath::KinChain& kc) */ Rmath::KinChain Rmath::KinChain::operator+(const Rmath::KinChain& kc) { + // Add the new set of joints to the container + std::map _joints; + std::map::const_iterator joint_iter = joints.begin(); + while (joint_iter != joints.end()) + { + Rmath::KinChain::Joint current_q( (joint_iter->second).id, + (joint_iter->second).col, + (joint_iter->second).value, + (joint_iter->second).max, + (joint_iter->second).min); + + // Indices are shifted by the size of the precedent chain + _joints.insert( std::pair (joint_iter->first, current_q) ); + ++joint_iter; + } + std::map::const_iterator kcJoint_iter = kc.joints.begin(); + while (kcJoint_iter != kc.joints.end()) + { + Rmath::KinChain::Joint current_q( (kcJoint_iter->second).id, + joints.size() + (kcJoint_iter->second).col, + (kcJoint_iter->second).value, + (kcJoint_iter->second).max, + (kcJoint_iter->second).min); + + // Indices are shifted by the size of the precedent chain + _joints.insert( std::pair (transformations.size() + kcJoint_iter->first, current_q) ); + ++kcJoint_iter; + } + // First insert copies of the Transform objects of *this std::vector transf; for(int i=0; i< transformations.size(); ++i) @@ -336,8 +347,9 @@ Rmath::KinChain Rmath::KinChain::operator+(const Rmath::KinChain& kc) for(int i=0; i clone() ); + // NOTE: any information on the joint bounds is lost (TO FIX. - return KinChain(transf); + return KinChain(transf, _joints); } /* @@ -349,4 +361,4 @@ std::ostream& Rmath::operator<<(std::ostream& out, const Rmath::KinChain& kc) kc.transformations.at(i)->print(out); return out; } - +// NOTE: any information on the joint bounds is lost (TO FIX. diff --git a/naoqi/modules/R2module/src/r2Module.cpp b/naoqi/modules/R2module/src/r2Module.cpp index bf24d7c..739d8da 100644 --- a/naoqi/modules/R2module/src/r2Module.cpp +++ b/naoqi/modules/R2module/src/r2Module.cpp @@ -17,9 +17,6 @@ */ #define INFO(x) std::cerr << "\033[22;34;1m" << "[r2module] " << x << "\033[0m" << std::endl; -//#define DEBUG_MODE -//#define TEST_KINCHAIN - namespace AL { @@ -90,19 +87,49 @@ R2Module::~R2Module() */ void R2Module::init() { - // Initialize the base kinematic chain (from Nao's left foot to body center) - ConfigReader theConfigReader( CONFIG_PATH + LEFT_LEG_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); - theConfigReader.storeJointsID(&jointID); - theKinChainLeftLeg = new Rmath::KinChain( theConfigReader, LLEG_CHAIN_BEGIN, LLEG_CHAIN_END ); - -#ifdef TEST_KINCHAIN - // Initialize the test kinematic chain (left arm) - ConfigReader theConfigReader_TMP( CONFIG_PATH + LEFT_ARM_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); - theKinChain_TMP = new Rmath::KinChain( theConfigReader_TMP, LARM_CHAIN_BEGIN, LARM_CHAIN_SIZE); -#endif + ConfigReader theConfigReader(CONFIG_PATH + JOINT_BOUNDS_CFG); + theConfigReader.storeJointsID(&jointID); + + // Initialize the base kinematic chain (from Nao's left foot to body center) + theConfigReader.setKinChain(CONFIG_PATH + LEFT_LEG_BASE_CFG); + theKinChain_LLBase = new Rmath::KinChain( theConfigReader ); + + // Initialize the base kinematic chain (from Nao's right foot to body center) + theConfigReader.setKinChain(CONFIG_PATH + RIGHT_LEG_BASE_CFG); + theKinChain_RLBase = new Rmath::KinChain( theConfigReader ); + + // Initialize the fixed transformation base-ankle + base_ankle << 0.0, 0.0, 1.0, BASE_ANKLE_X, + 0.0, -1.0, 0.0, BASE_ANKLE_Y, + 1.0, 0.0, 0.0, BASE_ANKLE_Z, + 0.0, 0.0, 0.0, 1.0; + + // initialize kinChain transofrmation w.r.t. CoM + theConfigReader.setKinChain(CONFIG_PATH + COM_HEAD_CFG); + CoM_Head = new Rmath::KinChain( theConfigReader ); + theConfigReader.setKinChain(CONFIG_PATH + COM_LEFT_ARM_CFG); + CoM_LeftArm = new Rmath::KinChain( theConfigReader ); + theConfigReader.setKinChain(CONFIG_PATH + COM_RIGHT_ARM_CFG); + CoM_RightArm = new Rmath::KinChain( theConfigReader ); + theConfigReader.setKinChain(CONFIG_PATH + COM_RIGHT_LEG_CFG); + CoM_RightLeg= new Rmath::KinChain( theConfigReader ); + theConfigReader.setKinChain(CONFIG_PATH + COM_LEFT_LEG_CFG); + CoM_LeftLeg = new Rmath::KinChain( theConfigReader ); + + // Initialize kinChains + theConfigReader.setKinChain(CONFIG_PATH + LEFT_LEG_CFG); + theKinChainLeftLeg = new Rmath::KinChain( theConfigReader ); + theConfigReader.setKinChain(CONFIG_PATH + RIGHT_LEG_CFG); + theKinChainRightLeg = new Rmath::KinChain( theConfigReader ); + theConfigReader.setKinChain(CONFIG_PATH + LEFT_ARM_CFG); + theKinChainLeftArm = new Rmath::KinChain( theConfigReader ); + theConfigReader.setKinChain(CONFIG_PATH + RIGHT_ARM_CFG); + theKinChainRightArm = new Rmath::KinChain( theConfigReader ); + theConfigReader.setKinChain(CONFIG_PATH + HEAD_CFG); + theKinChainHead = new Rmath::KinChain( theConfigReader ); #ifdef DEBUG_MODE - INFO("Initializing: building up base kinematic chain (left foot to body center)... "); + INFO("Initializing: building up kinematic chains... "); #endif /* ---------------------------------------- Proxies and threads initialization -------------------------------------------- */ @@ -169,42 +196,42 @@ void R2Module::init() INFO("Initializing: retrieving joint limits... "); #endif - /* ------------------------------ Other tasks: head (priority = 2), left arm (priority = 1) ------------------------------ */ + /* ------------------------------------------------- Other tasks -------------------------------------------------------- */ - // Extract joint bounds from the configuration file - ConfigReader headConfig( CONFIG_PATH + HEAD_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); - // Task initialization - Task* head = new Task(HEAD_TASK_DIM, HEAD_CHAIN_SIZE, HEAD_TASK_PRIORITY, headConfig, - HEAD_CHAIN_BEGIN, HEAD_CHAIN_END); - // Extract joint bounds from the configuration file - ConfigReader LpointingConfig( CONFIG_PATH + LEFT_ARM_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); // Task initialization - Task* Lpointing = new Task(LARM_TASK_DIM, LARM_CHAIN_SIZE, LARM_TASK_PRIORITY, LpointingConfig, - LARM_CHAIN_BEGIN, LARM_CHAIN_END); + Task* Rsupporting = new Task(RLEG_TASK_DIM, RLEG_CHAIN_SIZE, RLEG_TASK_PRIORITY, *theKinChainRightLeg); - // Extract joint bounds from the configuration file - ConfigReader RpointingConfig( CONFIG_PATH + RIGHT_ARM_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); - // Task initialization - Task* Rpointing = new Task(RARM_TASK_DIM, RARM_CHAIN_SIZE, RARM_TASK_PRIORITY, RpointingConfig, - RARM_CHAIN_BEGIN, RARM_CHAIN_END); + Task* Lsupporting = new Task(LLEG_TASK_DIM, LLEG_CHAIN_SIZE, LLEG_TASK_PRIORITY, *theKinChainLeftLeg); - // Push every task into the task map - taskMap.insert( std::pair ("Head task", head) ); - taskMap.insert( std::pair ("Left arm task", Lpointing) ); + Task* Lpointing = new Task(LARM_TASK_DIM, LARM_CHAIN_SIZE+LLEG_CHAIN_SIZE, + LARM_TASK_PRIORITY, *theKinChain_LLBase + *CoM_LeftArm + *theKinChainLeftArm, + LLEG_CHAIN_SIZE); + + Task* Rpointing = new Task(RARM_TASK_DIM, RARM_CHAIN_SIZE+LLEG_CHAIN_SIZE, + RARM_TASK_PRIORITY, *theKinChain_LLBase + *CoM_RightArm + *theKinChainRightArm, + LLEG_CHAIN_SIZE); + + Task* looking = new Task(HEAD_TASK_DIM, HEAD_CHAIN_SIZE+LLEG_CHAIN_SIZE, + HEAD_TASK_PRIORITY, *theKinChain_LLBase + *CoM_Head + *theKinChainHead, + LLEG_CHAIN_SIZE); + + // // Push every task into the task map + taskMap.insert( std::pair ("Head task", looking) ); taskMap.insert( std::pair ("Right arm task", Rpointing) ); + taskMap.insert( std::pair ("Left arm task", Lpointing) ); + taskMap.insert( std::pair ("Right leg task", Rsupporting) ); + taskMap.insert( std::pair ("Left leg task", Lsupporting) ); - // Storing tasks location with respect to the whole Nao kinematic chain - taskLoc.insert( std::pair (head, HEAD_CHAIN_BEGIN) ); - taskLoc.insert( std::pair (Lpointing, LARM_CHAIN_BEGIN) ); - taskLoc.insert( std::pair (Rpointing, RARM_CHAIN_BEGIN) ); #ifdef DEBUG_MODE INFO("Initializing tasks: "); INFO("- Joint limits, with priority " << jointLimits->getPriority()); - INFO("- Head task, with priority " << head->getPriority()); - INFO("- Left arm task, with priority " << Lpointing->getPriority()); + INFO("- Head task, with priority " << looking->getPriority()); INFO("- Right arm task, with priority " << Rpointing->getPriority()); + INFO("- Left arm task, with priority " << Lpointing->getPriority()); + INFO("- Right leg task, with priority " << Rsupporting->getPriority()); + INFO("- Left leg task, with priority " << Lsupporting->getPriority()); #endif } @@ -327,17 +354,30 @@ void R2Module::motion() INFO("Executing main loop..."); #endif - // Shift CoM-right arm - Eigen::Vector3d CoM_RA; - CoM_RA << COM_RARM_SHIFT_X, COM_RARM_SHIFT_Y, COM_RARM_SHIFT_Z; - // Shift CoM-left arm - Eigen::Vector3d CoM_LA; - CoM_LA << COM_LARM_SHIFT_X, COM_LARM_SHIFT_Y, COM_LARM_SHIFT_Z; +// // Turn tasks to active + jointLimits->activate(); + taskMap["Head task"]->activate(); + taskMap["Right arm task"]->activate(); + taskMap["Left arm task"]->activate(); + taskMap["Right leg task"]->activate(); + taskMap["Left leg task"]->activate(); // Defining the desired pose vectors in the task space - Eigen::VectorXd desiredHeadPose(HEAD_TASK_DIM), desiredLHandPose(LARM_TASK_DIM), desiredRHandPose(RARM_TASK_DIM); - Eigen::VectorXd desiredHeadPosition(3), desiredRHandPosition(3), desiredLHandPosition(3); - Eigen::VectorXd desiredHeadOrientation(3), desiredRHandOrientation(3), desiredLHandOrientation(3); + Eigen::VectorXd desiredHeadPose(HEAD_TASK_DIM), + desiredLHandPose(LARM_TASK_DIM), + desiredRHandPose(RARM_TASK_DIM), + desiredRLegPose(RLEG_TASK_DIM), + desiredLLegPose(LLEG_TASK_DIM); + Eigen::VectorXd desiredHeadPosition(3), + desiredRHandPosition(3), + desiredLHandPosition(3), + desiredRLegPosition(3), + desiredLLegPosition(3); + Eigen::VectorXd desiredHeadOrientation(3), + desiredRHandOrientation(3), + desiredLHandOrientation(3), + desiredRLegOrientation(3), + desiredLLegOrientation(3); // head position in 3D space desiredHeadPosition << HEAD_DESIRED_X, HEAD_DESIRED_Y, HEAD_DESIRED_Z; @@ -351,6 +391,14 @@ void R2Module::motion() desiredLHandPosition << LARM_DESIRED_X, LARM_DESIRED_Y, LARM_DESIRED_Z; // left arm orientation in 3D space desiredLHandOrientation << LARM_DESIRED_ROLL, LARM_DESIRED_PITCH, LARM_DESIRED_YAW; + // Right leg position in 3D space + desiredRLegPosition << RLEG_DESIRED_X, RLEG_DESIRED_Y, RLEG_DESIRED_Z; + // Right leg orientation in 3D space + desiredRLegOrientation << RLEG_DESIRED_ROLL, RLEG_DESIRED_PITCH, RLEG_DESIRED_YAW; + // Left leg position in 3D space + desiredLLegPosition << LLEG_DESIRED_X, LLEG_DESIRED_Y, LLEG_DESIRED_Z; + // Left leg orientation in 3D space + desiredLLegOrientation << LLEG_DESIRED_ROLL, LLEG_DESIRED_PITCH, LLEG_DESIRED_YAW; if( HEAD_TASK_DIM > 3 ) desiredHeadPose << desiredHeadPosition, desiredHeadOrientation.head(HEAD_TASK_DIM-3); @@ -367,43 +415,90 @@ void R2Module::motion() else desiredRHandPose << desiredRHandPosition.head(RARM_TASK_DIM); -// taskMap["Head task"]->setDesiredConfiguration(desiredHeadPose.head(HEAD_TASK_DIM), HEAD_TASK_NSTEPS); - taskMap["Head task"]->setDesiredPose(desiredHeadPose.head(HEAD_TASK_DIM), HEAD_TASK_NSTEPS, - Rmath::hTranslation(Eigen::Vector3d::Zero())); + if( RLEG_TASK_DIM > 3 ) + desiredRLegPose << desiredRLegPosition, desiredRLegOrientation.head(RLEG_TASK_DIM-3); + else + desiredRLegPose << desiredRLegPosition.head(RLEG_TASK_DIM); + + if( LLEG_TASK_DIM > 3 ) + desiredLLegPose << desiredLLegPosition, desiredLLegOrientation.head(LLEG_TASK_DIM-3); + else + desiredLLegPose << desiredLLegPosition.head(LLEG_TASK_DIM); + + Eigen::Vector3d iDesiredPose; + iDesiredPose << 0.0, 48, 590; + + Eigen::Matrix4d h_CoMRL; + CoM_RightLeg->forward(&h_CoMRL); + if(taskMap["Right leg task"]->isActive()) + taskMap["Right leg task"]->setDesiredPose( desiredRLegPose.head(RLEG_TASK_DIM), RLEG_TASK_NSTEPS, h_CoMRL ); + + Eigen::Matrix4d h_CoMLL; + CoM_LeftLeg->forward(&h_CoMLL); + if(taskMap["Left leg task"]->isActive()) + taskMap["Left leg task"]->setDesiredPose( desiredLLegPose.head(LLEG_TASK_DIM), LLEG_TASK_NSTEPS, h_CoMLL ); + + if(taskMap["Head task"]->isActive()) + taskMap["Head task"]->setDesiredPose( desiredHeadPose.head(HEAD_TASK_DIM), HEAD_TASK_NSTEPS, base_ankle ); + + if(taskMap["Left arm task"]->isActive()) +#ifndef LARM_CIRCLE_TASK + taskMap["Left arm task"]->setDesiredPose(iDesiredPose, desiredLHandPose.head(LARM_TASK_DIM), LARM_TASK_NSTEPS, base_ankle ); +#else + taskMap["Left arm task"]->circularPathGenerator(desiredLHandPose.head(LARM_TASK_DIM), CIRCLE_Z_DEPTH, + LARM_TASK_NSTEPS, CIRCLE_RADIUS, CIRCLE_LAPS, base_ankle ); +#endif - taskMap["Left arm task"]->setDesiredPose(desiredLHandPose.head(LARM_TASK_DIM), LARM_TASK_NSTEPS, - Rmath::homX(-M_PI/2, CoM_LA) ); -// taskMap["Left arm task"]->circularPathGenerator(desiredLHandPose.head(LARM_TASK_DIM), -70, LARM_TASK_NSTEPS, 100, 3, -// Rmath::homX(-M_PI/2, CoM_LA)); +#ifndef RARM_LARM_JOINT_TASK + if(taskMap["Right arm task"]->isActive()) +#ifndef RARM_CIRCLE_TASK + taskMap["Right arm task"]->setDesiredPose(desiredRHandPose.head(RARM_TASK_DIM), RARM_TASK_NSTEPS, base_ankle ); +#else + taskMap["Right arm task"]->circularPathGenerator(desiredRHandPose.head(RARM_TASK_DIM), CIRCLE_Z_DEPTH, + RARM_TASK_NSTEPS, CIRCLE_RADIUS, CIRCLE_LAPS, base_ankle ); +#endif +#endif -// taskMap["Right arm task"]->setDesiredPose(desiredRHandPose.head(RARM_TASK_DIM), RARM_TASK_NSTEPS, -// Rmath::homX(-M_PI/2, CoM_RA) ); - taskMap["Right arm task"]->circularPathGenerator(desiredRHandPose.head(RARM_TASK_DIM), -70, RARM_TASK_NSTEPS, 100, 3, - Rmath::homX(-M_PI/2, CoM_RA)); #ifdef DEBUG_MODE INFO("Desired head position in space: [\n" << desiredHeadPose << "]"); INFO("Desired left hand position in space: [\n" << desiredLHandPose << "]"); +#ifndef RARM_LARM_JOINT_TASK INFO("Desired right hand position in space: [\n" << desiredRHandPose << "]"); #endif + INFO("Desired right foot position in space: [\n" << desiredRLegPose << "]"); + INFO("Desired Hip position in space: [\n" << desiredLLegPose << "]"); +#endif - // Turn tasks to active - jointLimits->activate(); - taskMap["Head task"]->activate(); - taskMap["Left arm task"]->activate(); - taskMap["Right arm task"]->activate(); + updateConstraints(getConfiguration()); - closeHand("RHand"); - openHand("LHand"); // Main loop while(true) { +#ifdef UP_DOWN_TASK + if ( (taskMap["Right leg task"]->done()) && (taskMap["Left leg task"]->done()) ) + { + UP_DOWN *= -1; + + Eigen::Matrix4d h_CoMRL, h_CoMLL; + CoM_RightLeg->forward(&h_CoMRL); + CoM_LeftLeg->forward(&h_CoMLL); + + Eigen::VectorXd desiredRLegPose = taskMap["Right leg task"]->getTargetPose(); + Eigen::VectorXd desiredLLegPose = taskMap["Left leg task"]->getTargetPose(); + desiredRLegPose(2) += UP_DOWN; + desiredLLegPose(2) += UP_DOWN; + taskMap["Right leg task"]->setDesiredPose(desiredRLegPose, RLEG_TASK_NSTEPS, h_CoMRL ); + taskMap["Left leg task"]->setDesiredPose(desiredLLegPose, LLEG_TASK_NSTEPS, h_CoMLL ); + } +#endif + // Retrieve current robot configuration Eigen::VectorXd q(jointID.size()); q = getConfiguration(); #ifdef DEBUG_MODE - INFO("Current joint configuration:"); + INFO("\nCurrent joint configuration:"); for(unsigned int i=0; i::iterator updater = taskSet.begin(); while( updater != taskSet.end() ) { + // Retrieve current kinchain joints information + std::map jointsIndicesMap; + (*updater)->getJointsIDs(&jointsIndicesMap); // Current task constraint matrix (restricted to the joint involved) Eigen::MatrixXd currentA_task = (*updater)->constraintMatrix(); // Rewrite the task constraint matrix with the complete joint configuration Eigen::MatrixXd currentA = Eigen::MatrixXd::Zero( currentA_task.rows(), JOINTS_NUM ); // The task constraint matrix is the only non-zero block in A - currentA.block( 0, taskLoc[*updater], currentA_task.rows(), currentA_task.cols() ) = currentA_task; + for (unsigned int i = 0; i < jointID.size(); ++i) + { + if (jointsIndicesMap.find(jointID.at(i)) != jointsIndicesMap.end()) + currentA.col(i) = currentA_task.col( jointsIndicesMap[jointID.at(i)] ); + else + currentA.col(i) = Eigen::VectorXd::Zero(currentA_task.rows()); + } A.push_back(currentA); b.push_back((*updater)->vectorBounds()); @@ -476,19 +580,8 @@ void R2Module::motion() hsolver.showActiveSet(std::cerr); INFO("Solution: [\n" << qdot << "]"); #endif - -#ifdef TEST_KINCHAIN - qdot = Eigen::VectorXd::Zero(q.size()); - Eigen::MatrixXd J_TMP_pinv; - Rmath::pseudoInverse(J_TMP, &J_TMP_pinv); - - qdot.segment(LARM_CHAIN_BEGIN) = J_TMP_pinv * r; - INFO("Solution: [\n" << qdot << "]"); -#endif - // Update the robot configuration with the obtained solution updateConfiguration(q + qdot*TIME_STEP); - } } @@ -519,15 +612,6 @@ Eigen::VectorXd R2Module::getConfiguration() */ void R2Module::updateConstraints(const Eigen::VectorXd& q) { - // Updating the base kinematic chain (left foot to body center) - Eigen::Matrix4d H_base; - Eigen::MatrixXd J_base(6, LLEG_CHAIN_SIZE); - // Left leg joints are those on the bottom part of the IDs vector - theKinChainLeftLeg->update(q.tail(LLEG_CHAIN_SIZE)); - // Compute kinematics - theKinChainLeftLeg->forward(&H_base); - theKinChainLeftLeg->differential(&J_base); - /* ------------------------- Joint bounds update -------------------------*/ Eigen::MatrixXd velBounds(q.size(), 2); @@ -539,35 +623,15 @@ void R2Module::updateConstraints(const Eigen::VectorXd& q) INFO(std::endl << *jointLimits); #endif -#ifdef TEST_KINCHAIN - INFO("Updating test kinematic chain (body to left hand): "); - q_TMP << q.segment(LARM_CHAIN_BEGIN); - INFO("Joint configuration:"); - for(unsigned int i=0; iupdate(q_TMP); - theKinChain_TMP->forward(&H_TMP); - theKinChain_TMP->differential(&J_TMP, 3); - - INFO("Direct kinematics transform: "); - INFO(std::endl << H_TMP); - INFO("Jacobian (linear velocities only): "); - INFO(std::endl << J_TMP); -#endif - /* -------------------- Head and arms tasks update --------------------*/ - // Shift CoM-left arm - Eigen::Vector3d CoM_LA; - CoM_LA << 0.0, 98.0, 100.0; - // Shift CoM-right arm - Eigen::Vector3d CoM_RA; - CoM_RA << 0.0, -98.0, 100.0; // Head task update - ( taskMap["Head task"] )->update( q.head(HEAD_CHAIN_SIZE), Eigen::VectorXd::Zero(HEAD_TASK_DIM), K_HEAD, - Rmath::hTranslation(Eigen::Vector3d::Zero())); + Eigen::VectorXd q_H (LLEG_CHAIN_SIZE+HEAD_CHAIN_SIZE); + q_H << q.segment(LLEG_CHAIN_BEGIN), + q.segment(HEAD_CHAIN_BEGIN); + if(taskMap["Head task"]->isActive()) + ( taskMap["Head task"] )->update( q_H, Eigen::VectorXd::Zero(HEAD_TASK_DIM), K_HEAD, base_ankle ); #ifdef DEBUG_MODE INFO("Head task constraint equation: "); @@ -575,9 +639,11 @@ void R2Module::updateConstraints(const Eigen::VectorXd& q) #endif // Left arm task update - ( taskMap["Left arm task"] )->update( q.segment(LARM_CHAIN_BEGIN), Eigen::VectorXd::Zero(LARM_TASK_DIM), - K_LARM, Rmath::homX(-M_PI/2, CoM_LA) ); -// ( taskMap["Left arm task"] )->update( q.segment(LARM_CHAIN_BEGIN), Eigen::VectorXd::Zero(LARM_CHAIN_SIZE), K_LARM); + Eigen::VectorXd q_LA (LLEG_CHAIN_SIZE+LARM_CHAIN_SIZE); + q_LA << q.segment(LLEG_CHAIN_BEGIN), + q.segment(LARM_CHAIN_BEGIN); + if( taskMap["Left arm task"]->isActive() ) + ( taskMap["Left arm task"] )->update( q_LA, Eigen::VectorXd::Zero(LARM_TASK_DIM), K_LARM, base_ankle ); #ifdef DEBUG_MODE INFO("Left arm task constraint equation: "); @@ -585,22 +651,73 @@ void R2Module::updateConstraints(const Eigen::VectorXd& q) #endif // Right arm task update - ( taskMap["Right arm task"] )->update( q.segment(RARM_CHAIN_BEGIN), Eigen::VectorXd::Zero(RARM_TASK_DIM), - K_RARM, Rmath::homX(-M_PI/2, CoM_RA) ); -// ( taskMap["Right arm task"] )->update( q.segment(RARM_CHAIN_BEGIN), Eigen::VectorXd::Zero(RARM_CHAIN_SIZE), K_RARM); + Eigen::VectorXd q_RA (LLEG_CHAIN_SIZE+RARM_CHAIN_SIZE); + q_RA << q.segment(LLEG_CHAIN_BEGIN), + q.segment(RARM_CHAIN_BEGIN); +#ifdef RARM_LARM_JOINT_TASK + if ( ARMS_TASK == MIMIC_TASK ) + { + Eigen::VectorXd desiredRHandPose = ( taskMap["Left arm task"] )->getTargetPose(); + assert(RARM_TASK_DIM == LARM_TASK_DIM); + assert(LARM_TASK_DIM >= 2); + desiredRHandPose(1) = desiredRHandPose(1)-MINIMUM_HANDS_DISTANCE; + + if (LARM_TASK_DIM > 3) + for (int i=3; i < LARM_TASK_DIM; ++i) + desiredRHandPose(i) = -desiredRHandPose(i); + + ( taskMap["Right arm task"] )->setDesiredPose(desiredRHandPose, 1, base_ankle ); + ( taskMap["Right arm task"] )->update( q_RA, Eigen::VectorXd::Zero(RARM_TASK_DIM), K_RARM, base_ankle ); + } + else if ( ARMS_TASK == MIRROR_TASK ) + { + Eigen::VectorXd desiredRHandVel = ( taskMap["Left arm task"] )->getTargetVelocity(); + + assert(RARM_TASK_DIM == LARM_TASK_DIM); + assert(LARM_TASK_DIM >= 2); + desiredRHandVel(1) = -desiredRHandVel(1); + + if (LARM_TASK_DIM > 3) + for (int i=3; i < LARM_TASK_DIM; ++i) + desiredRHandVel(i) = -desiredRHandVel(i); + + ( taskMap["Right arm task"] )->update( q_RA, desiredRHandVel, K_RARM, base_ankle ); + } +#else + if( taskMap["Right arm task"]->isActive() ) + ( taskMap["Right arm task"] )->update( q_RA, Eigen::VectorXd::Zero(RARM_TASK_DIM), K_RARM, base_ankle ); +#endif #ifdef DEBUG_MODE INFO("Right arm task constraint equation: "); INFO(std::endl << *( taskMap["Right arm task"] ) ); #endif -#ifdef TEST_KINCHAIN - INFO("Desired left hand position in space: [\n" << desiredLHandPose << "]"); -// r = K_HEAD * ( desiredLHandPose - H_TMP.topRightCorner(3,1) ) + Eigen::Vector3d::UnitZ()*100; - r = ( Eigen::Vector3d::UnitX()*100 + - Eigen::Vector3d::UnitY()*100 + - Eigen::Vector3d::UnitZ()*100 ); - INFO("New target velocity:\n[" << r << "]"); + // Right leg task update + Eigen::Matrix4d h_CoMRL; + CoM_RightLeg->forward(&h_CoMRL); + if( taskMap["Right leg task"]->isActive() ) + ( taskMap["Right leg task"] )->update( q.segment(RLEG_CHAIN_BEGIN), + Eigen::VectorXd::Zero(RLEG_TASK_DIM), + K_RLEG, h_CoMRL); + +#ifdef DEBUG_MODE + INFO("Right leg task constraint equation: "); + INFO(std::endl << *( taskMap["Right leg task"] ) ); +#endif + + + // Left leg task update + Eigen::Matrix4d h_CoMLL; + CoM_LeftLeg->forward(&h_CoMLL); + if( taskMap["Left leg task"]->isActive() ) + ( taskMap["Left leg task"] )->update( q.segment(LLEG_CHAIN_BEGIN), + Eigen::VectorXd::Zero(LLEG_TASK_DIM), + K_LLEG, h_CoMLL ); + +#ifdef DEBUG_MODE + INFO("Left leg task constraint equation: "); + INFO(std::endl << *( taskMap["Left leg task"] ) ); #endif } diff --git a/naoqi/modules/R2module/src/r2Module_backUp.cpp b/naoqi/modules/R2module/src/r2Module_backUp.cpp new file mode 100644 index 0000000..1ab93d6 --- /dev/null +++ b/naoqi/modules/R2module/src/r2Module_backUp.cpp @@ -0,0 +1,834 @@ +#include +#include + +#include +#include +#include +#include + +#include "r2Module.h" +#include "configReader.h" +#include "configParams.h" + +/** + TODO: + - force soth convergence (stop criteria) + - task formalization +*/ + +#define INFO(x) std::cerr << "\033[22;34;1m" << "[r2module] " << x << "\033[0m" << std::endl; +//#define DEBUG_MODE +//#define TEST_KINCHAIN + + +namespace AL +{ + +/* + * Module constructor using Naoqi API methods. + */ +R2Module::R2Module( boost::shared_ptr pBroker, const std::string& pName ): + ALModule(pBroker , pName), fRegisteredToVideoDevice(false) +{ + // Module description + setModuleDescription("Robotics2 Project"); + + // Bound methods definition and description + functionName( "registerToVideoDevice", getName(), "Register to the V.I.M." ); + BIND_METHOD( R2Module::registerToVideoDevice ); + + functionName( "unRegisterFromVideoDevice", getName(), "Unregister from the V.I.M." ); + BIND_METHOD( R2Module::unRegisterFromVideoDevice ); + + functionName( "getCurrentFrame", getName(), "Save an image received from the camera." ); + BIND_METHOD( R2Module::getCurrentFrame ); + +#ifdef DEBUG_MODE + INFO("Module generated."); +#endif +} + +/* + * Module destructor: unsubscribes proxies, destroys threads and deallocates all memory used + */ +R2Module::~R2Module() +{ + try + { + // Unregister the video module + if(fCamProxy) fCamProxy->unsubscribe(fVideoClientName); + // Reset all proxies + fCamProxy.reset(); + fMotionProxy.reset(); + fPostureProxy.reset(); + } + catch(const AL::ALError& e) + { + qiLogError("vision.R2Module") << e.toString() << std::endl; + } + + // Destroy thread + pthread_cancel( motionThreadId ); + pthread_cancel( cameraThreadId ); + + // Deallocate memory + if (jointLimits) delete jointLimits; + // Cleaning up containers + std::map::iterator destroyer = taskMap.begin(); + while (destroyer != taskMap.end()) + { + if(destroyer->second) delete destroyer->second; + ++destroyer; + } + taskMap.clear(); + taskSet.clear(); +} + +/* + * Method automatically called when the module is created: + * Initializes threads, proxy interfaces along with the private members of the module itself. + */ +void R2Module::init() +{ + // Initialize the base kinematic chain (from Nao's left foot to body center) + ConfigReader theConfigReader( CONFIG_PATH + LEFT_LEG_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); + theConfigReader.storeJointsID(&jointID); + theKinChainLeftLeg = new Rmath::KinChain( theConfigReader, LLEG_CHAIN_BEGIN, LLEG_CHAIN_END ); + +#ifdef TEST_KINCHAIN + // Initialize the test kinematic chain (left arm) + ConfigReader theConfigReader_TMP( CONFIG_PATH + LEFT_ARM_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); + theKinChain_TMP = new Rmath::KinChain( theConfigReader_TMP, LARM_CHAIN_BEGIN, LARM_CHAIN_SIZE); +#endif + +#ifdef DEBUG_MODE + INFO("Initializing: building up base kinematic chain (left foot to body center)... "); +#endif + + /* ---------------------------------------- Proxies and threads initialization -------------------------------------------- */ + + // Create a proxy for ALVideoDevice, ALMotion and ALRobotPosture + try + { + fCamProxy = boost::shared_ptr(new ALVideoDeviceProxy(getParentBroker())); + fMotionProxy = boost::shared_ptr(new ALMotionProxy(getParentBroker())); + fPostureProxy = boost::shared_ptr(new ALRobotPostureProxy(getParentBroker())); + fBallTrackerProxy = boost::shared_ptr(new ALRedBallTrackerProxy(getParentBroker())); + } + catch (const AL::ALError& e) + { + qiLogError("vision.R2Module") << "Error while getting proxy on ALVideoDevice. Error msg: " << e.toString() << std::endl; + R2Module::exit(); + return; + } + + // Check whether the proxy has been correctly initialized + if(!fCamProxy || !fMotionProxy || !fPostureProxy) + { + qiLogError("vision.AmrModule") << "Error while getting proxy on ALVideoDevice. Check ALVideoDevice is running." << std::endl; + R2Module::exit(); + return; + } + + // Initialize resolution, image size and color space + pResolution = AL::kQVGA; + pColorSpace = AL::kYuvColorSpace; + int imgWidth = 0, imgHeight = 0; + setSizeFromResolution(pResolution, imgWidth, imgHeight); + // Subscrive to the camera + registerToVideoDevice(); + if(fRegisteredToVideoDevice) + { + // Initialize image + fcurrImageHeader = cv::Mat::zeros(imgHeight, imgWidth, CV_8UC1); + + // Create the camera thread + pthread_create( &cameraThreadId, NULL, (void*(*)(void*)) visionTh, this); + // Create the motion thread + pthread_create( &motionThreadId, NULL, (void*(*)(void*)) motionTh, this); + +#ifdef DEBUG_MODE + INFO("Proxies and threads correctly initialized..."); +#endif + + /* -------------------------- First and foremost task (top priority): joint limits -------------------------------------- */ + + + // Extract joint bounds from the configuration file + jointBounds = Eigen::MatrixXd::Zero(JOINTS_NUM, 2); + theConfigReader.extractJointBounds(&jointBounds); + + // Convert joint ranges into velocity bound estimates + Eigen::MatrixXd velBounds(JOINTS_NUM, 2); + posBound2velBound(jointBounds, getConfiguration(), &velBounds); + + // Actual task initialization + jointLimits = new TaskBase(0, Eigen::MatrixXd::Identity( JOINTS_NUM, JOINTS_NUM ), velBounds); + +#ifdef DEBUG_MODE + INFO("Initializing: retrieving joint limits... "); +#endif + + /* ------------------------------ Other tasks: head (priority = 2), left arm (priority = 1) ------------------------------ */ + + // Extract joint bounds from the configuration file + ConfigReader headConfig( CONFIG_PATH + HEAD_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); + // Task initialization + Task* head = new Task(HEAD_TASK_DIM, HEAD_CHAIN_SIZE, HEAD_TASK_PRIORITY, headConfig, + HEAD_CHAIN_BEGIN, HEAD_CHAIN_END); + + // Extract joint bounds from the configuration file + ConfigReader LpointingConfig( CONFIG_PATH + LEFT_ARM_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); + // Task initialization + Task* Lpointing = new Task(LARM_TASK_DIM, LARM_CHAIN_SIZE, LARM_TASK_PRIORITY, LpointingConfig, + LARM_CHAIN_BEGIN, LARM_CHAIN_END); + + // Extract joint bounds from the configuration file + ConfigReader RpointingConfig( CONFIG_PATH + RIGHT_ARM_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); + // Task initialization + Task* Rpointing = new Task(RARM_TASK_DIM, RARM_CHAIN_SIZE, RARM_TASK_PRIORITY, RpointingConfig, + RARM_CHAIN_BEGIN, RARM_CHAIN_END); + + // Extract joint bounds from the configuration file + ConfigReader RlegConfig( CONFIG_PATH + RIGHT_LEG_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); + // Task initialization + Task* Rsupporting = new Task(RLEG_TASK_DIM, RLEG_CHAIN_SIZE, RLEG_TASK_PRIORITY, RlegConfig, + RLEG_CHAIN_BEGIN, RLEG_CHAIN_END); + + // Extract joint bounds from the configuration file + ConfigReader LlegConfig( CONFIG_PATH + LEFT_LEG_CFG, CONFIG_PATH + JOINT_BOUNDS_CFG ); + // Task initialization + Task* Lsupporting = new Task(LLEG_TASK_DIM, LLEG_CHAIN_SIZE, LLEG_TASK_PRIORITY, LlegConfig, + LLEG_CHAIN_BEGIN, LLEG_CHAIN_END); + + // Push every task into the task map + taskMap.insert( std::pair ("Head task", head) ); + taskMap.insert( std::pair ("Right arm task", Rpointing) ); + taskMap.insert( std::pair ("Left arm task", Lpointing) ); + taskMap.insert( std::pair ("Right leg task", Rsupporting) ); + taskMap.insert( std::pair ("Left leg task", Lsupporting) ); + + // Storing tasks location with respect to the whole Nao kinematic chain + taskLoc.insert( std::pair (head, HEAD_CHAIN_BEGIN) ); + taskLoc.insert( std::pair (Rpointing, RARM_CHAIN_BEGIN) ); + taskLoc.insert( std::pair (Lpointing, LARM_CHAIN_BEGIN) ); + taskLoc.insert( std::pair (Rsupporting, RLEG_CHAIN_BEGIN) ); + taskLoc.insert( std::pair (Lsupporting, LLEG_CHAIN_BEGIN) ); + +#ifdef DEBUG_MODE + INFO("Initializing tasks: "); + INFO("- Joint limits, with priority " << jointLimits->getPriority()); + INFO("- Head task, with priority " << head->getPriority()); + INFO("- Right arm task, with priority " << Rpointing->getPriority()); + INFO("- Left arm task, with priority " << Lpointing->getPriority()); + INFO("- Right leg task, with priority " << Rsupporting->getPriority()); + INFO("- Left leg task, with priority " << Lsupporting>getPriority()); +#endif + } + +} + +/* + * Method automatically called when the module is destroyed. + */ +void R2Module::exit() +{ + AL::ALModule::exit(); +} + +/* + * Register/unregister to the camera proxy. + */ +void R2Module::registerToVideoDevice() +{ + // Check if another module has subscribed already + if (fRegisteredToVideoDevice) + { + throw ALError(getName(), "registerToVideoDevice()", "A video module has already been " + "registered. Call unRegisterFromVideoDevice() before trying to register a new module."); + } + // Release any image header previously allocated. + if (!fcurrImageHeader.empty()) fcurrImageHeader.release(); + + if(fCamProxy) + fVideoClientName = fCamProxy->subscribeCamera("r2Module", 1, pResolution, pColorSpace, 30); + qiLogInfo("vision.r2Module") << "Module registered as " << fVideoClientName << std::endl; + + // Registration successful, set flag to true + fRegisteredToVideoDevice = true; +} + +void R2Module::unRegisterFromVideoDevice() +{ + // Check if the module is actually registered + if (!fRegisteredToVideoDevice) + { + throw ALError(getName(), "unRegisterFromVideoDevice()", "No video module is currently " + "registered! Call registerToVideoDevice first."); + } + // Release any image header previously allocated. + if (!fcurrImageHeader.empty()) fcurrImageHeader.release(); + + qiLogInfo("vision.R2Module") << "Unregister " << fVideoClientName << " module..." << std::endl; + if(fCamProxy) + fCamProxy->unsubscribe(fVideoClientName); + qiLogInfo("vision.R2Module") << "Done." << std::endl; + + // Unregistration successful, set flag to false. + fRegisteredToVideoDevice = false; +} + +/* + * Retrieve the current camera image. + */ +void R2Module::getCurrentFrame() +{ + // Check if a video module is actually registered + if (!fRegisteredToVideoDevice) + throw ALError(getName(), "saveImageRemote()", "No video module is currently " + "registered! Call registerToVideoDevice() first."); + + // Image request via the camera proxy + ALValue frame = fCamProxy->getImageRemote(fVideoClientName); + + // Check for correctness of the camera frame + if (frame.getType()!= ALValue::TypeArray && frame.getSize() != 7) + throw ALError(getName(), "getImages", "Invalid image returned."); + + // Put a time stamp on the image header + const long long timeStamp = ((long long)(int)frame[4])*1000000LL + ((long long)(int)frame[5]); + time = (int)(timeStamp/1000000LL); + // Retrieve image data + fcurrImageHeader.data = (uchar*) frame[6].GetBinary(); + + // Prompt the image in a window + if ((char) cv::waitKey(33) != 27) + cv::imshow("Frame", fcurrImageHeader); + + // Release the proxy + fCamProxy->releaseImage(fVideoClientName); +} + +/* + * The vision process. + */ +void R2Module::vision() +{ +// fBallTrackerProxy->startTracker(); + // Wait a while before starting acquisition + qi::os::sleep(1.5); + // Main loop + while(true) + { + qi::os::msleep(100); + /* WORK IN PROGRESS */ +// INFO("ball position w.r.t. CoM: \n"<stopTracker(); +} + +/* + * The motion process: + * - At each time step, the current robot configuration is retrieved to update all the robot tasks; + * - Active tasks are then pushed into a priority-ordered set, so to generate a task hierarchy for the HQP solver; + * - Once the solver is configured, active search is enabled and a feasible solution is sought; + * - The solution obtained for q_dot is integrated in discrete time and eventually used to update the robot configuration. + */ +void R2Module::motion() +{ + // Initialize the posture proxy + fPostureProxy->goToPosture("StandInit", 0.5f); + +#ifdef DEBUG_MODE + INFO("Posture proxy initialized."); + INFO("Turning tasks to 'active'... "); + INFO("Executing main loop..."); +#endif + + // Shift CoM-right arm + Eigen::Vector3d CoM_RA; + CoM_RA << COM_RARM_SHIFT_X, COM_RARM_SHIFT_Y, COM_RARM_SHIFT_Z; + // Shift CoM-left arm + Eigen::Vector3d CoM_LA; + CoM_LA << COM_LARM_SHIFT_X, COM_LARM_SHIFT_Y, COM_LARM_SHIFT_Z; + + // Shift CoM-right leg + Eigen::Vector3d CoM_RL; + CoM_RL << COM_RLEG_SHIFT_X, COM_RLEG_SHIFT_Y, COM_RLEG_SHIFT_Z; + // Shift CoM-left leg + Eigen::Vector3d Base_Lankle; + Base_Lankle << BASE_LANKLE_SHIFT_X, BASE_LANKLE_SHIFT_Y, BASE_LANKLE_SHIFT_Z; + + // Defining the desired pose vectors in the task space + Eigen::VectorXd desiredHeadPose(HEAD_TASK_DIM), desiredLHandPose(LARM_TASK_DIM), desiredRHandPose(RARM_TASK_DIM), + desiredRLegPose(RLEG_TASK_DIM), desiredLLegPose(LLEG_TASK_DIM); + Eigen::VectorXd desiredHeadPosition(3), desiredRHandPosition(3), desiredLHandPosition(3), + desiredRLegPosition(3), desiredLLegPosition(3); + Eigen::VectorXd desiredHeadOrientation(3), desiredRHandOrientation(3), desiredLHandOrientation(3), + desiredRLegOrientation(3), desiredLLegOrientation(3); + + // head position in 3D space + desiredHeadPosition << HEAD_DESIRED_X, HEAD_DESIRED_Y, HEAD_DESIRED_Z; + // head orientation in 3D space + desiredHeadOrientation << HEAD_DESIRED_ROLL, HEAD_DESIRED_PITCH, HEAD_DESIRED_YAW; + // Right arm position in 3D space + desiredRHandPosition << RARM_DESIRED_X, RARM_DESIRED_Y, RARM_DESIRED_Z; + // Right arm orientation in 3D space + desiredRHandOrientation << RARM_DESIRED_ROLL, RARM_DESIRED_PITCH, RARM_DESIRED_YAW; + // left arm position in 3D space + desiredLHandPosition << LARM_DESIRED_X, LARM_DESIRED_Y, LARM_DESIRED_Z; + // left arm orientation in 3D space + desiredLHandOrientation << LARM_DESIRED_ROLL, LARM_DESIRED_PITCH, LARM_DESIRED_YAW; + // Right leg position in 3D space + desiredRLegPosition << RLEG_DESIRED_X, RLEG_DESIRED_Y, RLEG_DESIRED_Z; + // Right leg orientation in 3D space + desiredRLegOrientation << RLEG_DESIRED_ROLL, RLEG_DESIRED_PITCH, RLEG_DESIRED_YAW; + // Left leg position in 3D space + desiredLLegPosition << LLEG_DESIRED_X, LLEG_DESIRED_Y, LLEG_DESIRED_Z; + // Left leg orientation in 3D space + desiredLLegOrientation << LLEG_DESIRED_ROLL, LLEG_DESIRED_PITCH, LLEG_DESIRED_YAW; + + if( HEAD_TASK_DIM > 3 ) + desiredHeadPose << desiredHeadPosition, desiredHeadOrientation.head(HEAD_TASK_DIM-3); + else + desiredHeadPose << desiredHeadPosition.head(HEAD_TASK_DIM); + + if( LARM_TASK_DIM > 3 ) + desiredLHandPose << desiredLHandPosition, desiredLHandOrientation.head(LARM_TASK_DIM-3); + else + desiredLHandPose << desiredLHandPosition.head(LARM_TASK_DIM); + + if( RARM_TASK_DIM > 3 ) + desiredRHandPose << desiredRHandPosition, desiredRHandOrientation.head(RARM_TASK_DIM-3); + else + desiredRHandPose << desiredRHandPosition.head(RARM_TASK_DIM); + + if( RLEG_TASK_DIM > 3 ) + desiredRLegPose << desiredRLegPosition, desiredRLegOrientation.head(RLEG_TASK_DIM-3); + else + desiredRLegPose << desiredRLegPosition.head(RLEG_TASK_DIM); + + if( LLEG_TASK_DIM > 3 ) + desiredLLegPose << desiredLLegPosition, desiredLLegOrientation.head(LLEG_TASK_DIM-3); + else + desiredLLegPose << desiredLLegPosition.head(LLEG_TASK_DIM); + + taskMap["Right leg task"]->setDesiredPose(desiredRLegPose.head(RLEG_TASK_DIM), RLEG_TASK_NSTEPS, + Rmath::homX(-M_PI/4, CoM_RL) ); + +// taskMap["Head task"]->setDesiredConfiguration(desiredHeadPose.head(HEAD_TASK_DIM), HEAD_TASK_NSTEPS); +// taskMap["Head task"]->setDesiredPose(desiredHeadPose.head(HEAD_TASK_DIM), HEAD_TASK_NSTEPS, +// Rmath::hTranslation(Eigen::Vector3d::Zero())); + +// taskMap["Left arm task"]->setDesiredPose(desiredLHandPose.head(LARM_TASK_DIM), LARM_TASK_NSTEPS, +// Rmath::homX(-M_PI/2, CoM_LA) ); +// taskMap["Left arm task"]->circularPathGenerator(desiredLHandPose.head(LARM_TASK_DIM), -70, LARM_TASK_NSTEPS, 100, 3, +// Rmath::homX(-M_PI/2, CoM_LA)); + +// taskMap["Right arm task"]->setDesiredPose(desiredRHandPose.head(RARM_TASK_DIM), RARM_TASK_NSTEPS, +// Rmath::homX(-M_PI/2, CoM_RA) ); +// taskMap["Right arm task"]->circularPathGenerator(desiredRHandPose.head(RARM_TASK_DIM), -70, RARM_TASK_NSTEPS, 100, 3, +// Rmath::homX(-M_PI/2, CoM_RA)); + +#ifdef DEBUG_MODE + INFO("Desired head position in space: [\n" << desiredHeadPose << "]"); + INFO("Desired left hand position in space: [\n" << desiredLHandPose << "]"); + INFO("Desired right hand position in space: [\n" << desiredRHandPose << "]"); +#endif + + // Turn tasks to active + jointLimits->activate(); +// taskMap["Head task"]->activate(); +// taskMap["Left arm task"]->activate(); +// taskMap["Right arm task"]->activate(); + taskMap["Right leg task"]->activate(); + + + closeHand("RHand"); + openHand("LHand"); + bool init = true; + // Main loop + while(true) + { +// if(init) +// { +// INFO("starting pointing task... "); +// taskMap["Left arm task"]->setDesiredPose(desiredLHandPose.head(LARM_TASK_DIM), +// LARM_TASK_NSTEPS, +// Rmath::homX(-M_PI/2, CoM_LA) ); +//// taskMap["Right arm task"]->circularPathGenerator(desiredRHandPose.head(RARM_TASK_DIM), -70, +//// RARM_TASK_NSTEPS, 100, 3, +//// Rmath::homX(-M_PI/2, CoM_RA)); +// init = false; +// } +// if( taskMap["Left arm task"]->isConverged() ) +// init = true; + +// if(init) +// { +// INFO("starting circular path... "); +// taskMap["Left arm task"]->circularPathGenerator(desiredLHandPose.head(LARM_TASK_DIM), -50, +// LARM_TASK_NSTEPS, 70, 3, +// Rmath::homX(-M_PI/2, CoM_LA)); +// init=false; +// } + + + // Retrieve current robot configuration + Eigen::VectorXd q(jointID.size()); + q = getConfiguration(); + +#ifdef DEBUG_MODE + INFO("Current joint configuration:"); + for(unsigned int i=0; i::const_iterator adder; + for ( adder = taskMap.begin(); adder != taskMap.end(); ++adder ) + if( (adder->second)->isActive()) + taskSet.insert(adder->second); + + // Initialize the HQP solver + soth::HCOD hsolver( q.size(), taskSet.size()+1 ); + Eigen::VectorXd qdot(q.size()); + +#ifdef DEBUG_MODE + INFO("Initializing HQP solver..."); + INFO( "Active tasks: " << (taskSet.size()+1) << " out of " << (taskMap.size()+1) ); + INFO("Generating stack..."); + + int task_i = 2; +#endif + // Set up the hierarchy + std::vector A; + std::vector b; + // Joint limits first (top priority task) + A.push_back(jointLimits->constraintMatrix()); + b.push_back(jointLimits->vectorBounds()); + // All the remaining tasks (in descending order of priority) + std::set::iterator updater = taskSet.begin(); + while( updater != taskSet.end() ) + { + // Current task constraint matrix (restricted to the joint involved) + Eigen::MatrixXd currentA_task = (*updater)->constraintMatrix(); + + // Rewrite the task constraint matrix with the complete joint configuration + Eigen::MatrixXd currentA = Eigen::MatrixXd::Zero( currentA_task.rows(), JOINTS_NUM ); + // The task constraint matrix is the only non-zero block in A + currentA.block( 0, taskLoc[*updater], currentA_task.rows(), currentA_task.cols() ) = currentA_task; + + A.push_back(currentA); + b.push_back((*updater)->vectorBounds()); + ++updater; + +#ifdef DEBUG_MODE + INFO("Task n. " << task_i << ", constraint matrix (enlarged):\n" << currentA); + ++task_i; +#endif + } + hsolver.pushBackStages(A, b); + + // Configure the solver + hsolver.setDamping(0.0); + hsolver.setInitialActiveSet(); + +#ifdef DEBUG_MODE + INFO("Configuring HQP solver..."); + INFO("Start active search... "); +#endif + // Run the solver + hsolver.activeSearch(qdot); + // Trim ssolution + Rmath::trim(&qdot); + +#ifdef DEBUG_MODE + INFO("Done."); + INFO("Active set: "); + hsolver.showActiveSet(std::cerr); + INFO("Solution: [\n" << qdot << "]"); +#endif + +#ifdef TEST_KINCHAIN + qdot = Eigen::VectorXd::Zero(q.size()); + Eigen::MatrixXd J_TMP_pinv; + Rmath::pseudoInverse(J_TMP, &J_TMP_pinv); + + qdot.segment(LARM_CHAIN_BEGIN) = J_TMP_pinv * r; + INFO("Solution: [\n" << qdot << "]"); +#endif + + // Update the robot configuration with the obtained solution + updateConfiguration(q + qdot*TIME_STEP); + + } +} + +/* + * Retrieve the current joint configuration with the motion proxy. + */ +Eigen::VectorXd R2Module::getConfiguration() +{ + // Store the resulting values in a Eigen::VectorXd + Eigen::VectorXd currentConfiguration(jointID.size()); + + AL::ALValue jointId, angles; + + // Use Naoqi ID for each joint to request the joint current measured value + for(int i=0; i getAngles( jointId, true ); + + for(int i=0; i update(q.tail(LLEG_CHAIN_SIZE)); + // Compute kinematics + theKinChainLeftLeg->forward(&H_base); + theKinChainLeftLeg->differential(&J_base); + + /*fast DEBUG*/ + INFO( std::endl << *taskMap["Right leg task"]->kinChain() ); + Eigen::Matrix4d H_RL; + (taskMap["Right leg task"]->kinChain())->forward(&H_RL); + INFO("H right Leg \n"<< H_RL); + + /* ------------------------- Joint bounds update -------------------------*/ + + Eigen::MatrixXd velBounds(q.size(), 2); + posBound2velBound(jointBounds, q, &velBounds); + jointLimits->setVectorBounds(velBounds); + +#ifdef DEBUG_MODE + INFO("Updating joint bounds: "); + INFO(std::endl << *jointLimits); +#endif + +#ifdef TEST_KINCHAIN + INFO("Updating test kinematic chain (body to left hand): "); + q_TMP << q.segment(LARM_CHAIN_BEGIN); + INFO("Joint configuration:"); + for(unsigned int i=0; iupdate(q_TMP); + theKinChain_TMP->forward(&H_TMP); + theKinChain_TMP->differential(&J_TMP, 3); + + INFO("Direct kinematics transform: "); + INFO(std::endl << H_TMP); + INFO("Jacobian (linear velocities only): "); + INFO(std::endl << J_TMP); +#endif + + /* -------------------- Head and arms tasks update --------------------*/ + + // Shift CoM-right arm + Eigen::Vector3d CoM_RA; + CoM_RA << COM_RARM_SHIFT_X, COM_RARM_SHIFT_Y, COM_RARM_SHIFT_Z; + // Shift CoM-left arm + Eigen::Vector3d CoM_LA; + CoM_LA << COM_LARM_SHIFT_X, COM_LARM_SHIFT_Y, COM_LARM_SHIFT_Z; + + // Shift CoM-right leg + Eigen::Vector3d CoM_RL; + CoM_RL << COM_RLEG_SHIFT_X, COM_RLEG_SHIFT_Y, COM_RLEG_SHIFT_Z; + // Shift CoM-left leg + Eigen::Vector3d Base_Lankle; + Base_Lankle << BASE_LANKLE_SHIFT_X, BASE_LANKLE_SHIFT_Y, BASE_LANKLE_SHIFT_Z; + + // Head task update + ( taskMap["Head task"] )->update( q.head(HEAD_CHAIN_SIZE), Eigen::VectorXd::Zero(HEAD_TASK_DIM), K_HEAD, + Rmath::hTranslation(Eigen::Vector3d::Zero())); + +#ifdef DEBUG_MODE + INFO("Head task constraint equation: "); + INFO(std::endl << *( taskMap["Head task"] ) ); +#endif + + // Left arm task update + ( taskMap["Left arm task"] )->update( q.segment(LARM_CHAIN_BEGIN), Eigen::VectorXd::Zero(LARM_TASK_DIM), + K_LARM, Rmath::homX(-M_PI/2, CoM_LA) ); +// ( taskMap["Left arm task"] )->update( q.segment(LARM_CHAIN_BEGIN), Eigen::VectorXd::Zero(LARM_CHAIN_SIZE), K_LARM); + +#ifdef DEBUG_MODE + INFO("Left arm task constraint equation: "); + INFO(std::endl << *( taskMap["Left arm task"] ) ); +#endif + + // Right arm task update + ( taskMap["Right arm task"] )->update( q.segment(RARM_CHAIN_BEGIN), Eigen::VectorXd::Zero(RARM_TASK_DIM), + K_RARM, Rmath::homX(-M_PI/2, CoM_RA) ); +// ( taskMap["Right arm task"] )->update( q.segment(RARM_CHAIN_BEGIN), Eigen::VectorXd::Zero(RARM_CHAIN_SIZE), K_RARM); + +#ifdef DEBUG_MODE + INFO("Right arm task constraint equation: "); + INFO(std::endl << *( taskMap["Right arm task"] ) ); +#endif + + // Right leg task update + ( taskMap["Right leg task"] )->update( q.segment(RLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(RLEG_TASK_DIM), + K_RLEG, Rmath::homX(-M_PI/4, CoM_RL) ); +// ( taskMap["Right leg task"] )->update( q.segment(RLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(RLEG_CHAIN_SIZE), K_RLEG); + +#ifdef DEBUG_MODE + INFO("Right arm task constraint equation: "); + INFO(std::endl << *( taskMap["Right leg task"] ) ); +#endif + +#ifdef TEST_KINCHAIN + INFO("Desired left hand position in space: [\n" << desiredLHandPose << "]"); +// r = K_HEAD * ( desiredLHandPose - H_TMP.topRightCorner(3,1) ) + Eigen::Vector3d::UnitZ()*100; + r = ( Eigen::Vector3d::UnitX()*100 + + Eigen::Vector3d::UnitY()*100 + + Eigen::Vector3d::UnitZ()*100 ); + INFO("New target velocity:\n[" << r << "]"); +#endif + +} + +/* + * Call Naoqi motion proxy to actually execute the motion. + */ +bool R2Module::updateConfiguration(const Eigen::VectorXd& delta_q) +{ + +#ifdef TEST_KINCHAIN + Eigen::VectorXd delta_q_ = delta_q; +#endif + + // Initialize variables + AL::ALValue jointId, angles, setStiff, zeroArray; + // Push into the vector all joint value increments to be requested + for(int i=0; i< delta_q.size(); ++i) + { + +#ifdef TEST_KINCHAIN + if (delta_q_(i) > 0) + { + if ( delta_q_(i) > jointBounds(i,1) ) + delta_q_(i) = jointBounds(i,1); + } + else + { + if ( delta_q_(i) < jointBounds(i,0) ) + delta_q_(i) = jointBounds(i,0); + } + + if(delta_q_(i) != 0) + { + jointId.arrayPush( jointID.at(i) ); + angles.arrayPush( delta_q_(i) ); + setStiff.arrayPush( 1.0 ); + zeroArray.arrayPush( 0.0 ); + } + continue; +#endif + if(delta_q(i) != 0) + { + jointId.arrayPush( jointID.at(i) ); +// angles.arrayPush( delta_q(i) ); + + if(jointID.at(i) == "RHipYawPitch") angles.arrayPush( /*delta_q(i)*/-M_PI/2 ); + else if(jointID.at(i) == "RHipRoll") angles.arrayPush( /*delta_q(i)*/-M_PI/4 ); + else if(jointID.at(i) == "RHipPitch") angles.arrayPush( /*delta_q(i)*/0.0 ); + else if(jointID.at(i) == "RKneePitch") angles.arrayPush( /*delta_q(i)*/0.0 ); + else if(jointID.at(i) == "RAnklePitch") angles.arrayPush( /*delta_q(i)*/0.0 ); + else if(jointID.at(i) == "RAnkleRoll") angles.arrayPush( /*delta_q(i)*/0.0 ); + else + angles.arrayPush( 0.0 ); + + setStiff.arrayPush( 1.0 ); + zeroArray.arrayPush( 0.0 ); + } + } + // Set up joint stiffness + fMotionProxy->stiffnessInterpolation( jointId, setStiff, 0.3f ); + fMotionProxy->setStiffnesses(jointId, setStiff); + + // Request motion to the motion proxy + fMotionProxy->setAngles(jointId, angles, 0.3f); + + // Reset stiffness + fMotionProxy->setStiffnesses(jointId, zeroArray); +} + +/* + * Estimate the joint velocity bounds given the actual joint limits and the current configuration. + */ +void R2Module::posBound2velBound( const Eigen::MatrixXd& posBound, const Eigen::VectorXd& configuration, Eigen::MatrixXd* velBound) +{ + Eigen::MatrixXd currConf(configuration.size(), 2); + currConf << configuration, configuration; + + *velBound = (posBound - currConf)/TIME_STEP; +} + +void R2Module::closeHand(const std::string& handID) +{ + if(handID == "both") + { + fMotionProxy->setStiffnesses("LHand", 1.0); + fMotionProxy->setStiffnesses("RHand", 1.0); + fMotionProxy->closeHand("LHand"); + fMotionProxy->closeHand("RHand"); + fMotionProxy->setStiffnesses("LHand", 0.0); + fMotionProxy->setStiffnesses("RHand", 0.0); + } + else + { + fMotionProxy->setStiffnesses(handID, 1.0); + fMotionProxy->closeHand(handID); + fMotionProxy->setStiffnesses(handID, 0.0); + } + +} + +void R2Module::openHand(const std::string& handID) +{ + if(handID == "both") + { + fMotionProxy->setStiffnesses("LHand", 1.0); + fMotionProxy->setStiffnesses("RHand", 1.0); + fMotionProxy->openHand("LHand"); + fMotionProxy->openHand("RHand"); + fMotionProxy->setStiffnesses("LHand", 0.0); + fMotionProxy->setStiffnesses("RHand", 0.0); + } + else + { + fMotionProxy->setStiffnesses(handID, 1.0); + fMotionProxy->openHand(handID); + fMotionProxy->setStiffnesses(handID, 0.0); + } + +} + +Eigen::Vector3d R2Module::getRedBallPosition() +{ + Eigen::Vector3d ballPosition; + ballPosition << fBallTrackerProxy->getPosition().at(0), + fBallTrackerProxy->getPosition().at(1), + fBallTrackerProxy->getPosition().at(2); + + return ballPosition; +} + +} // namespace AL diff --git a/naoqi/modules/R2module/src/task.cpp b/naoqi/modules/R2module/src/task.cpp index 1655162..3faa0cd 100644 --- a/naoqi/modules/R2module/src/task.cpp +++ b/naoqi/modules/R2module/src/task.cpp @@ -232,12 +232,31 @@ TaskBase TaskBase::operator--(int) * - A ConfigReader class object to initialize the task kinematic chain; * - Two indices _base, _ee identifying the task kinematic chain in the ConfigReader object file. */ -Task::Task(int m, int n, int _priority, ConfigReader theConfigReader, int _base, int _ee): +Task::Task(int m, int n, int _priority, ConfigReader theConfigReader): // Base class initalization TaskBase(_priority, Eigen::MatrixXd::Identity(m, n), Eigen::VectorXd::Zero(m), soth::Bound::BOUND_TWIN ), // Kinematic chain initialization - theKinChain( new Rmath::KinChain( theConfigReader, _base, _ee) ), - positioningActive(false), jointControlActive(false) + theKinChain( new Rmath::KinChain(theConfigReader) ), base_end(0), + // Flag initialization + positioningActive(false), jointControlActive(false), + // Target velocit initialization + targetVelocity(Eigen::VectorXd::Zero(m)) +{ +#ifdef DEBUG_MODE + INFO("Task kinematic chain:"); + INFO(theKinChain); +#endif +} + +Task::Task(int m, int n, int _priority, const Rmath::KinChain& _kc, int _base): + // Base class initalization + TaskBase(_priority, Eigen::MatrixXd::Identity(m, n), Eigen::VectorXd::Zero(m), soth::Bound::BOUND_TWIN ), + // Kinematic chain initialization + theKinChain( new Rmath::KinChain(_kc) ), base_end(_base), + // Flag initialization + positioningActive(false), jointControlActive(false), + // Target velocit initialization + targetVelocity(Eigen::VectorXd::Zero(m)) { #ifdef DEBUG_MODE INFO("Task kinematic chain:"); @@ -271,6 +290,15 @@ Eigen::VectorXd Task::getCurrentPose(const Eigen::Matrix4d& baseTransform) const return currentPose; } +/* TOCOMMENT */ +const Eigen::VectorXd Task::getTargetPose() const +{ + if(positioningActive) + return path.at(path_currentStep); + else + return getCurrentPose(); +} + void Task::setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints, const Eigen::Matrix4d& baseTransform) { // Re-computing direct kinematics @@ -290,6 +318,7 @@ void Task::setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints, const assert(dp.size() == initialPose.size()); + path.clear(); for (float i = 1.0; i <= n_controlPoints; ++i) path.push_back(initialPose + (i/static_cast(n_controlPoints)) * (dp - initialPose)); @@ -298,14 +327,48 @@ void Task::setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints, const if (jointControlActive) jointControlActive = false; } +void Task::setDesiredPose(const Eigen::VectorXd& idp, const Eigen::VectorXd& dp, int n_controlPoints, const Eigen::Matrix4d& baseTransform) +{ + // Re-computing direct kinematics + Eigen::Matrix4d H_chain; + theKinChain->forward((&H_chain)); + H_chain = baseTransform * H_chain; + + Eigen::VectorXd initialPose (constraint_matrix.rows()); + if(constraint_matrix.rows() > POSITION_TASK_DIM) + // Retrieving the position from the translation vector of the forward kinematics + // Retrieving the orientation from the Euler fixed frame x-y-z angles + initialPose << H_chain.topRightCorner(POSITION_TASK_DIM,1), + Rmath::xyzEulerAngles( H_chain.topLeftCorner(3,3) ).head(constraint_matrix.rows()-POSITION_TASK_DIM); + // If the task target is a position vector (A row size <= 3) just the translation vector of the forward kinematics is needed + else + initialPose << H_chain.col(POSITION_TASK_DIM).head(constraint_matrix.rows()); + + assert(idp.size() == initialPose.size()); + assert(dp.size() == initialPose.size()); + + path.clear(); + for (float i = 1.0; i <= n_controlPoints; ++i) + path.push_back(initialPose + (i/static_cast(n_controlPoints)) * (idp - initialPose)); + + for (float i = 1.0; i <= n_controlPoints; ++i) + path.push_back(idp + (i/static_cast(n_controlPoints)) * (dp - idp)); + + path_currentStep = 0; + positioningActive = true; + if (jointControlActive) jointControlActive = false; +} + void Task::setDesiredConfiguration(const Eigen::VectorXd& desiredConf, int n_controlPoints) { + Eigen::VectorXd initialConf = theKinChain->jointConfiguration(); assert(initialConf.size() == desiredConf.size()); constraint_matrix = Eigen::MatrixXd::Identity(initialConf.size(), initialConf.size()); bounds.setZero(initialConf.size(),1); + path.clear(); for (float i = 1.0; i <= n_controlPoints; ++i) path.push_back(initialConf + (i/static_cast(n_controlPoints)) * (desiredConf - initialConf)); @@ -317,28 +380,17 @@ void Task::setDesiredConfiguration(const Eigen::VectorXd& desiredConf, int n_con void Task::circularPathGenerator( const Eigen::VectorXd& dp, float z_shift, int n_controlPoints, float radius, int n, const Eigen::Matrix4d& baseTransform ) { + // Re-computing direct kinematics Eigen::Matrix4d H_chain; theKinChain->forward((&H_chain)); H_chain = baseTransform * H_chain; -// Eigen::Vector3d initialTranslation; -// initialTranslation << 218.7, 113, 112.31; -// Eigen::Vector3d transEEd = dp.head(3) - initialTranslation; -// INFO("desired vector: \n" << dp.head(3)); -// INFO("current vector: \n" << H_chain.topRightCorner(3,1)); -// /* very beginning Nao left hand translation -// 218.7, 113, 112.31 -// */ -// INFO("translation vector: \n" << transEEd); - + path.clear(); for (float i = 0.0; i < n*2*M_PI; i+= 2*M_PI/n_controlPoints) { Eigen::VectorXd ee_desiredPose_handframe(4); - ee_desiredPose_handframe << /*transEEd(0)+*/ radius*cos(i), - /*transEEd(1)+*/ radius*sin(i), - /*transEEd(2)+*/ z_shift, - 1.0; + ee_desiredPose_handframe << radius*cos(i), radius*sin(i), z_shift, 1.0; Rmath::trim(&ee_desiredPose_handframe); Eigen::VectorXd ee_desiredPose_CoMframe(dp.size()); @@ -361,11 +413,17 @@ void Task::circularPathGenerator( const Eigen::VectorXd& dp, float z_shift, int * Task update function: update the constraint matrix A=A(q) with a new value of q and replace the target vector * with a proportional/derivative control law of the type K*POSE_ERROR + DESIRED_VELOCITY. */ -void Task::update( const Eigen::VectorXd& q, const Eigen::VectorXd& desiredVel, double K, const Eigen::Matrix4d& baseTransform ) +void Task::update( const Eigen::VectorXd& _q, const Eigen::VectorXd& desiredVel, double K, const Eigen::Matrix4d& baseTransform ) { // Dimensions must be consistent assert(constraint_matrix.rows() == desiredVel.size()); - assert(constraint_matrix.cols() == q.size()); + assert(constraint_matrix.cols() == _q.size()); + + Eigen::VectorXd q(_q.size()); + q << -_q.head(base_end).reverse(), _q.tail(_q.size()-base_end); + + Eigen::Matrix4d H_chain; + theKinChain->forward((&H_chain)); // Cartesian space task if (!jointControlActive) @@ -421,21 +479,33 @@ void Task::update( const Eigen::VectorXd& q, const Eigen::VectorXd& desiredVel, else currentPose << H_chain.col(POSITION_TASK_DIM).head(constraint_matrix.rows()); - Eigen::VectorXd pose_error = path.at(path_currentStep) - currentPose; + Eigen::VectorXd pose_error; + if(path_currentStep < path.size()) + pose_error = path.at(path_currentStep) - currentPose; + else + pose_error = path.at(path.size()-1) - currentPose; // Updating the bound vector with the task error + a feedforward term for(int i=0; ijointConfiguration(); + Eigen::VectorXd joint_error; + if(path_currentStep < path.size()) + joint_error = path.at(path_currentStep) - theKinChain->jointConfiguration(); + else + joint_error = path.at(path.size()-1) - theKinChain->jointConfiguration(); // Updating the bound vector with the task error + a feedforward term for(int i=0; i