Commit a80b2418 authored by Paul Dandignac's avatar Paul Dandignac
Browse files

Rename different task states

parent 024ef4e8
Pipeline #1753 passed with stage
in 27 minutes and 5 seconds
...@@ -228,17 +228,17 @@ namespace dynamicgraph { ...@@ -228,17 +228,17 @@ namespace dynamicgraph {
enum RightHandState enum RightHandState
{ {
HIGH_RIGHT_HAND = 0, TASK_RIGHT_HAND_ON = 0,
/*HIGH_RIGHT_HAND_TRANSITION = 1,*/ /*TASK_RIGHT_HAND_TRANSITION = 1,*/
LOW_RIGHT_HAND = 1 TASK_RIGHT_HAND_OFF = 1
}; };
RightHandState m_rightHandState; RightHandState m_rightHandState;
enum LeftHandState enum LeftHandState
{ {
HIGH_LEFT_HAND = 0, TASK_LEFT_HAND_ON = 0,
/*HIGH_LEFT_HAND_TRANSITION = 1,*/ /*TASK_LEFT_HAND_TRANSITION = 1,*/
LOW_LEFT_HAND = 1 TASK_LEFT_HAND_OFF = 1
}; };
LeftHandState m_leftHandState; LeftHandState m_leftHandState;
/*double m_handsTransitionTime;*/ /// end time of the current transition (if any) /*double m_handsTransitionTime;*/ /// end time of the current transition (if any)
......
...@@ -319,8 +319,8 @@ namespace dynamicgraph ...@@ -319,8 +319,8 @@ namespace dynamicgraph
,m_firstTime(true) ,m_firstTime(true)
,m_timeLast(0) ,m_timeLast(0)
,m_contactState(DOUBLE_SUPPORT) ,m_contactState(DOUBLE_SUPPORT)
,m_rightHandState(LOW_RIGHT_HAND) ,m_rightHandState(TASK_RIGHT_HAND_OFF)
,m_leftHandState(LOW_LEFT_HAND) ,m_leftHandState(TASK_LEFT_HAND_OFF)
,m_robot_util(RefVoidRobotUtil()) ,m_robot_util(RefVoidRobotUtil())
{ {
RESETDEBUG5(); RESETDEBUG5();
...@@ -430,7 +430,7 @@ namespace dynamicgraph ...@@ -430,7 +430,7 @@ namespace dynamicgraph
void InverseDynamicsBalanceController::addTaskRightHand(/*const double& transitionTime*/) 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); SEND_MSG("Adds right hand SE3 task in "/*+toString(transitionTime)+" s"*/, MSG_TYPE_INFO);
const double & w_hands = m_w_handsSIN.accessCopy(); const double & w_hands = m_w_handsSIN.accessCopy();
...@@ -438,17 +438,17 @@ namespace dynamicgraph ...@@ -438,17 +438,17 @@ namespace dynamicgraph
} }
/*if(transitionTime>m_dt) /*if(transitionTime>m_dt)
{ {
m_rightHandState = HIGH_RIGHT_HAND_TRANSITION; m_rightHandState = TASK_RIGHT_HAND_TRANSITION;
m_handsTransitionTime = m_t + transitionTime; m_handsTransitionTime = m_t + transitionTime;
} }
else else
m_rightHandState = HIGH_RIGHT_HAND;*/ m_rightHandState = TASK_RIGHT_HAND_ON;*/
m_rightHandState = HIGH_RIGHT_HAND; m_rightHandState = TASK_RIGHT_HAND_ON;
} }
void InverseDynamicsBalanceController::addTaskLeftHand(/*const double& transitionTime*/) 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); SEND_MSG("Raise left hand in "/*+toString(transitionTime)+" s"*/, MSG_TYPE_INFO);
const double & w_hands = m_w_handsSIN.accessCopy(); const double & w_hands = m_w_handsSIN.accessCopy();
...@@ -456,12 +456,12 @@ namespace dynamicgraph ...@@ -456,12 +456,12 @@ namespace dynamicgraph
} }
/*if(transitionTime>m_dt) /*if(transitionTime>m_dt)
{ {
m_leftHandState = HIGH_LEFT_HAND_TRANSITION; m_leftHandState = TASK_LEFT_HAND_TRANSITION;
m_handsTransitionTime = m_t + transitionTime; m_handsTransitionTime = m_t + transitionTime;
} }
else else
m_leftHandState = HIGH_LEFT_HAND;*/ m_leftHandState = TASK_LEFT_HAND_ON;*/
m_leftHandState = HIGH_LEFT_HAND; m_leftHandState = TASK_LEFT_HAND_ON;
} }
void InverseDynamicsBalanceController::addRightFootContact(const double& transitionTime) void InverseDynamicsBalanceController::addRightFootContact(const double& transitionTime)
...@@ -488,21 +488,21 @@ namespace dynamicgraph ...@@ -488,21 +488,21 @@ namespace dynamicgraph
void InverseDynamicsBalanceController::removeTaskRightHand(const double& transitionTime) 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); SEND_MSG("Removes right hand SE3 task in "+toString(transitionTime)+" s", MSG_TYPE_INFO);
m_invDyn->removeTask(m_taskRH->name(), transitionTime); m_invDyn->removeTask(m_taskRH->name(), transitionTime);
m_rightHandState = LOW_RIGHT_HAND; m_rightHandState = TASK_RIGHT_HAND_OFF;
} }
} }
void InverseDynamicsBalanceController::removeTaskLeftHand(const double& transitionTime) 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); SEND_MSG("Removes left hand SE3 task in "+toString(transitionTime)+" s", MSG_TYPE_INFO);
m_invDyn->removeTask(m_taskLH->name(), transitionTime); m_invDyn->removeTask(m_taskLH->name(), transitionTime);
m_leftHandState = LOW_LEFT_HAND; m_leftHandState = TASK_LEFT_HAND_OFF;
} }
} }
...@@ -753,13 +753,13 @@ namespace dynamicgraph ...@@ -753,13 +753,13 @@ namespace dynamicgraph
{ {
m_contactState = LEFT_SUPPORT; 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); getProfiler().start(PROFILE_READ_INPUT_SIGNALS);
...@@ -826,7 +826,7 @@ namespace dynamicgraph ...@@ -826,7 +826,7 @@ namespace dynamicgraph
m_taskLF->Kp(kp_feet); m_taskLF->Kp(kp_feet);
m_taskLF->Kd(kd_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 Eigen::Matrix<double,12,1>& x_rh_ref = m_rh_ref_posSIN(iter);
const Vector6& dx_rh_ref = m_rh_ref_velSIN(iter); const Vector6& dx_rh_ref = m_rh_ref_velSIN(iter);
...@@ -840,7 +840,7 @@ namespace dynamicgraph ...@@ -840,7 +840,7 @@ namespace dynamicgraph
m_taskRH->Kp(kp_hands); m_taskRH->Kp(kp_hands);
m_taskRH->Kd(kd_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 Eigen::Matrix<double,12,1>& x_lh_ref = m_lh_ref_posSIN(iter);
const Vector6& dx_lh_ref = m_lh_ref_velSIN(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