Commit a80b2418 authored by Paul Dandignac's avatar Paul Dandignac

Rename different task states

parent 024ef4e8
......@@ -228,17 +228,17 @@ namespace dynamicgraph {
enum RightHandState
{
HIGH_RIGHT_HAND = 0,
/*HIGH_RIGHT_HAND_TRANSITION = 1,*/
LOW_RIGHT_HAND = 1
TASK_RIGHT_HAND_ON = 0,
/*TASK_RIGHT_HAND_TRANSITION = 1,*/
TASK_RIGHT_HAND_OFF = 1
};
RightHandState m_rightHandState;
enum LeftHandState
{
HIGH_LEFT_HAND = 0,
/*HIGH_LEFT_HAND_TRANSITION = 1,*/
LOW_LEFT_HAND = 1
TASK_LEFT_HAND_ON = 0,
/*TASK_LEFT_HAND_TRANSITION = 1,*/
TASK_LEFT_HAND_OFF = 1
};
LeftHandState m_leftHandState;
/*double m_handsTransitionTime;*/ /// end time of the current transition (if any)
......
......@@ -319,8 +319,8 @@ namespace dynamicgraph
,m_firstTime(true)
,m_timeLast(0)
,m_contactState(DOUBLE_SUPPORT)
,m_rightHandState(LOW_RIGHT_HAND)
,m_leftHandState(LOW_LEFT_HAND)
,m_rightHandState(TASK_RIGHT_HAND_OFF)
,m_leftHandState(TASK_LEFT_HAND_OFF)
,m_robot_util(RefVoidRobotUtil())
{
RESETDEBUG5();
......@@ -430,7 +430,7 @@ namespace dynamicgraph
void InverseDynamicsBalanceController::addTaskRightHand(/*const double& transitionTime*/)
{
if(m_rightHandState == LOW_RIGHT_HAND)
if(m_rightHandState == TASK_RIGHT_HAND_OFF)
{
SEND_MSG("Adds right hand SE3 task in "/*+toString(transitionTime)+" s"*/, MSG_TYPE_INFO);
const double & w_hands = m_w_handsSIN.accessCopy();
......@@ -438,17 +438,17 @@ namespace dynamicgraph
}
/*if(transitionTime>m_dt)
{
m_rightHandState = HIGH_RIGHT_HAND_TRANSITION;
m_rightHandState = TASK_RIGHT_HAND_TRANSITION;
m_handsTransitionTime = m_t + transitionTime;
}
else
m_rightHandState = HIGH_RIGHT_HAND;*/
m_rightHandState = HIGH_RIGHT_HAND;
m_rightHandState = TASK_RIGHT_HAND_ON;*/
m_rightHandState = TASK_RIGHT_HAND_ON;
}
void InverseDynamicsBalanceController::addTaskLeftHand(/*const double& transitionTime*/)
{
if(m_leftHandState == LOW_LEFT_HAND)
if(m_leftHandState == TASK_LEFT_HAND_OFF)
{
SEND_MSG("Raise left hand in "/*+toString(transitionTime)+" s"*/, MSG_TYPE_INFO);
const double & w_hands = m_w_handsSIN.accessCopy();
......@@ -456,12 +456,12 @@ namespace dynamicgraph
}
/*if(transitionTime>m_dt)
{
m_leftHandState = HIGH_LEFT_HAND_TRANSITION;
m_leftHandState = TASK_LEFT_HAND_TRANSITION;
m_handsTransitionTime = m_t + transitionTime;
}
else
m_leftHandState = HIGH_LEFT_HAND;*/
m_leftHandState = HIGH_LEFT_HAND;
m_leftHandState = TASK_LEFT_HAND_ON;*/
m_leftHandState = TASK_LEFT_HAND_ON;
}
void InverseDynamicsBalanceController::addRightFootContact(const double& transitionTime)
......@@ -488,21 +488,21 @@ namespace dynamicgraph
void InverseDynamicsBalanceController::removeTaskRightHand(const double& transitionTime)
{
if(m_rightHandState == HIGH_RIGHT_HAND)
if(m_rightHandState == TASK_RIGHT_HAND_ON)
{
SEND_MSG("Removes right hand SE3 task in "+toString(transitionTime)+" s", MSG_TYPE_INFO);
m_invDyn->removeTask(m_taskRH->name(), transitionTime);
m_rightHandState = LOW_RIGHT_HAND;
m_rightHandState = TASK_RIGHT_HAND_OFF;
}
}
void InverseDynamicsBalanceController::removeTaskLeftHand(const double& transitionTime)
{
if(m_leftHandState == HIGH_LEFT_HAND)
if(m_leftHandState == TASK_LEFT_HAND_ON)
{
SEND_MSG("Removes left hand SE3 task in "+toString(transitionTime)+" s", MSG_TYPE_INFO);
m_invDyn->removeTask(m_taskLH->name(), transitionTime);
m_leftHandState = LOW_LEFT_HAND;
m_leftHandState = TASK_LEFT_HAND_OFF;
}
}
......@@ -753,13 +753,13 @@ namespace dynamicgraph
{
m_contactState = LEFT_SUPPORT;
}
/*if(m_rightHandState == HIGH_RIGHT_HAND_TRANSITION && m_t >= m_handsTransitionTime)
/*if(m_rightHandState == TASK_RIGHT_HAND_TRANSITION && m_t >= m_handsTransitionTime)
{
m_rightHandState = HIGH_RIGHT_HAND;
m_rightHandState = TASK_RIGHT_HAND_ON;
}
if(m_leftHandState == HIGH_LEFT_HAND_TRANSITION && m_t >= m_handsTransitionTime)
if(m_leftHandState == TASK_LEFT_HAND_TRANSITION && m_t >= m_handsTransitionTime)
{
m_leftHandState = HIGH_LEFT_HAND;
m_leftHandState = TASK_LEFT_HAND_ON;
}*/
getProfiler().start(PROFILE_READ_INPUT_SIGNALS);
......@@ -826,7 +826,7 @@ namespace dynamicgraph
m_taskLF->Kp(kp_feet);
m_taskLF->Kd(kd_feet);
}
if(m_rightHandState == HIGH_RIGHT_HAND /*|| m_rightHandState == HIGH_RIGHT_HAND_TRANSITION*/)
if(m_rightHandState == TASK_RIGHT_HAND_ON /*|| m_rightHandState == TASK_RIGHT_HAND_TRANSITION*/)
{
const Eigen::Matrix<double,12,1>& x_rh_ref = m_rh_ref_posSIN(iter);
const Vector6& dx_rh_ref = m_rh_ref_velSIN(iter);
......@@ -840,7 +840,7 @@ namespace dynamicgraph
m_taskRH->Kp(kp_hands);
m_taskRH->Kd(kd_hands);
}
if(m_leftHandState == HIGH_LEFT_HAND /*|| m_leftHandState == HIGH_LEFT_HAND_TRANSITION*/)
if(m_leftHandState == TASK_LEFT_HAND_ON /*|| m_leftHandState == TASK_LEFT_HAND_TRANSITION*/)
{
const Eigen::Matrix<double,12,1>& x_lh_ref = m_lh_ref_posSIN(iter);
const Vector6& dx_lh_ref = m_lh_ref_velSIN(iter);
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment