Skip to content

Commit

Permalink
R2: r2module base module
Browse files Browse the repository at this point in the history
  • Loading branch information
@francescoriccio
francescoriccio committed Dec 17, 2013
1 parent a17ec3d commit 08b1f9e
Show file tree
Hide file tree
Showing 7 changed files with 955 additions and 527 deletions.
59 changes: 49 additions & 10 deletions naoqi/modules/R2module/include/configParams.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,9 @@

#include <Eigen/Core>

#define DEBUG_MODE
//#define DEBUG_MODE
//#define TASK_DEBUG
//#define TASK_MANAGER_DEBUG


// Discrete integration time step
Expand All @@ -30,8 +32,8 @@ static const double TIME_STEP = 1.0;


// Config (.cfg) files directory and paths
//static const std::string CONFIG_PATH = "/home/claudio/Naoqi/NaoModules/r2module/config/";
static const std::string CONFIG_PATH = "/home/francesco/naoqi/modules/R2module/config/";
//static const std::string CONFIG_PATH = "/home/claudio/Naoqi/NaoModules/r2module/config/";
// Joint parameters
static const std::string JOINT_BOUNDS_CFG = "joints_params.cfg";
// Base kinematic chains
Expand Down Expand Up @@ -92,7 +94,14 @@ static Eigen::Matrix4d base_ankle;

/** ---------------------------- Task parameters ---------------------------- */

static const float ACTIVATION_STEP = 1.00;

// Task names
static const std::string HEAD_TASK = "Head task";
static const std::string RIGHT_ARM = "Right arm task";
static const std::string LEFT_ARM = "Left arm task";
static const std::string RIGHT_LEG = "Right leg task";
static const std::string LEFT_LEG = "Left leg task";

// Task dimensions
static const int HEAD_TASK_DIM = 6;
Expand All @@ -104,7 +113,7 @@ static const int LLEG_TASK_DIM = 6;
// Task priorities
static const int HEAD_TASK_PRIORITY = 5;
static const int RARM_TASK_PRIORITY = 3;
static const int LARM_TASK_PRIORITY = 2;
static const int LARM_TASK_PRIORITY = 3;
static const int RLEG_TASK_PRIORITY = 1;
static const int LLEG_TASK_PRIORITY = 1;

Expand All @@ -117,25 +126,31 @@ static const double K_LLEG = 0.8;

// Control points number
static const int HEAD_TASK_NSTEPS = 1;
static const int RARM_TASK_NSTEPS = 30;
static const int LARM_TASK_NSTEPS = 30;
static const int RARM_TASK_NSTEPS = 15;
static const int LARM_TASK_NSTEPS = 8;
static const int RLEG_TASK_NSTEPS = 30;
static const int LLEG_TASK_NSTEPS = 30;

// Smooth transition step
static const double HEAD_TRANSITION_STEP = 1.0;
static const double RARM_TRANSITION_STEP = 1.0;
static const double LARM_TRANSITION_STEP = 1.0;
static const double RLEG_TRANSITION_STEP = 1.0;
static const double LLEG_TRANSITION_STEP = 1.0;

#define RARM_LARM_JOINT_TASK
//#define RARM_LARM_JOINT_TASK
enum TaskType {
MIRROR_TASK,
MIMIC_TASK
};
static const TaskType ARMS_TASK = MIRROR_TASK;
static const double MINIMUM_HANDS_DISTANCE = 350.0;
static const double MINIMUM_HANDS_DISTANCE = 100.0;

// Circle trajectory task parameters
//#define LARM_CIRCLE_TASK
//#define RARM_CIRCLE_TASK
static const double CIRCLE_Z_DEPTH = -70.0; // wrt ee-frame
static const double CIRCLE_RADIUS = 70.0;
static const double CIRCLE_RADIUS = 80.0;
static const int CIRCLE_LAPS = 10;

// Up-down with legs task
Expand All @@ -146,7 +161,11 @@ static int UP_DOWN = 80.0;

/** ------------------------------- Desired poses ------------------------------- */


static Eigen::VectorXd desiredHeadPose(6);
static Eigen::VectorXd desiredRHandPose(6);
static Eigen::VectorXd desiredLHandPose(6);
static Eigen::VectorXd desiredRLegPose(6);
static Eigen::VectorXd desiredLLegPose(6);

//Head
static const double HEAD_DESIRED_X = 0.0; // [mm]
Expand All @@ -168,7 +187,7 @@ static const double LARM_DESIRED_Y = -HIP_PELVIS_SHIFT+TORSO_SHOULDER_Y+ELBOW_SH
static const double LARM_DESIRED_Z = FOOT_HEIGHT+HIP_HEIGHT+HIP_TORSO+TORSO_SHOULDER_Z+WRIST_SHIFT_Z-9.9;
static const double LARM_DESIRED_ROLL = 0.0;
static const double LARM_DESIRED_PITCH = 0.0;
static const double LARM_DESIRED_YAW = M_PI_2;
static const double LARM_DESIRED_YAW = 0.0;
// Right leg
static const double RLEG_DESIRED_X = 20.0;
static const double RLEG_DESIRED_Y = -HIP_PELVIS_SHIFT;
Expand All @@ -193,4 +212,24 @@ static const double LLEG_DESIRED_PITCH = 0.0;
static const double LLEG_DESIRED_YAW = 0.0;


static void initialization()
{
// Initialize the fixed transformation base-ankle
base_ankle << 0.0, 0.0, 1.0, BASE_ANKLE_X,
0.0, -1.0, 0.0, BASE_ANKLE_Y,
1.0, 0.0, 0.0, BASE_ANKLE_Z,
0.0, 0.0, 0.0, 1.0;

desiredHeadPose << HEAD_DESIRED_X, HEAD_DESIRED_Y, HEAD_DESIRED_Z,
HEAD_DESIRED_ROLL, HEAD_DESIRED_PITCH, HEAD_DESIRED_YAW;
desiredRHandPose << RARM_DESIRED_X, RARM_DESIRED_Y, RARM_DESIRED_Z,
RARM_DESIRED_ROLL, RARM_DESIRED_PITCH, RARM_DESIRED_YAW;
desiredLHandPose << LARM_DESIRED_X, LARM_DESIRED_Y, LARM_DESIRED_Z,
LARM_DESIRED_ROLL, LARM_DESIRED_PITCH, LARM_DESIRED_YAW;
desiredRLegPose << RLEG_DESIRED_X, RLEG_DESIRED_Y, RLEG_DESIRED_Z,
RLEG_DESIRED_ROLL, RLEG_DESIRED_PITCH, RLEG_DESIRED_YAW;
desiredLLegPose << LLEG_DESIRED_X, LLEG_DESIRED_Y, LLEG_DESIRED_Z,
LLEG_DESIRED_ROLL, LLEG_DESIRED_PITCH, LLEG_DESIRED_YAW;
}

#endif
80 changes: 69 additions & 11 deletions naoqi/modules/R2module/include/r2Module.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,6 @@
#include <alproxies/alvideodeviceproxy.h>
#include <alproxies/alredballtrackerproxy.h>

// Soth includes
#include <soth/HCOD.hpp>

// Libmath includes
#include "libmath/kinChain.h"
#include "libmath/Rutils.h"
Expand Down Expand Up @@ -71,13 +68,76 @@ class R2Module : public AL::ALModule
std::vector<std::string> jointID;
// Container for the jointBounds
Eigen::MatrixXd jointBounds;
// Joint limits primary task
TaskBase* jointLimits;

// All initialized tasks
std::map <std::string, Task*> taskMap;
// Stack of tasks currently active
std::set<Task*, taskCmp> taskSet;
// Task handler and scheduler
class TaskManager
{

private:

// Pointer to the robot module
R2Module* module_ptr;

// Joint limits primary task
TaskBase* jointLimits;
// All initialized tasks
std::map <std::string, Task*> taskMap;
// Stack of tasks currently active
std::set<Task*, taskCmp> taskSet;
// Set of tasks changing their priority value
std::map<std::string, int> 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<std::string, Task*>(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<std::string,int>(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;
Expand Down Expand Up @@ -125,8 +185,6 @@ class R2Module : public AL::ALModule

// Retrieve the current joint configuration
Eigen::VectorXd getConfiguration();
// Update every active task with the current joint configuration
void updateConstraints(const Eigen::VectorXd& q);
// Call Naoqi motion proxy to actually execute the motion
bool updateConfiguration(const Eigen::VectorXd &delta_q);

Expand Down
Loading

0 comments on commit 08b1f9e

Please sign in to comment.