Commit bce95073 authored by Noëlie Ramuzat's avatar Noëlie Ramuzat
Browse files

Merge remote-tracking branch 'origin/devel_ddp' into devel_ddp

parents 3a707a0f dcc8e99f
...@@ -231,17 +231,17 @@ namespace dynamicgraph { ...@@ -231,17 +231,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)
......
...@@ -50,6 +50,36 @@ ...@@ -50,6 +50,36 @@
#define ODEBUG4FULL(x) #define ODEBUG4FULL(x)
#define ODEBUG4(x) #define ODEBUG4(x)
#include <ddp-actuator-solver/examples/dctemp.hh>
#include <ddp-actuator-solver/examples/costtemp.hh>
#if DEBUG
#define ODEBUG(x) std::cout << x << std::endl
#else
#define ODEBUG(x)
#endif
#define ODEBUG3(x) std::cout << x << std::endl
#define DBGFILE "/tmp/debug-ddp_actuator_solver.dat"
#define RESETDEBUG5() { std::ofstream DebugFile; \
DebugFile.open(DBGFILE,std::ofstream::out); \
DebugFile.close();}
#define ODEBUG5FULL(x) { std::ofstream DebugFile; \
DebugFile.open(DBGFILE,std::ofstream::app); \
DebugFile << __FILE__ << ":" \
<< __FUNCTION__ << "(#" \
<< __LINE__ << "):" << x << std::endl; \
DebugFile.close();}
#define ODEBUG5(x) { std::ofstream DebugFile; \
DebugFile.open(DBGFILE,std::ofstream::app); \
DebugFile << x << std::endl; \
DebugFile.close();}
#define RESETDEBUG4()
#define ODEBUG4FULL(x)
#define ODEBUG4(x)
namespace dynamicgraph namespace dynamicgraph
{ {
namespace sot namespace sot
......
...@@ -322,8 +322,8 @@ namespace dynamicgraph ...@@ -322,8 +322,8 @@ namespace dynamicgraph
,m_contactState(DOUBLE_SUPPORT) ,m_contactState(DOUBLE_SUPPORT)
,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();
...@@ -433,7 +433,7 @@ namespace dynamicgraph ...@@ -433,7 +433,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();
...@@ -441,17 +441,17 @@ namespace dynamicgraph ...@@ -441,17 +441,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();
...@@ -459,12 +459,12 @@ namespace dynamicgraph ...@@ -459,12 +459,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)
...@@ -493,21 +493,21 @@ namespace dynamicgraph ...@@ -493,21 +493,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;
} }
} }
...@@ -758,13 +758,13 @@ namespace dynamicgraph ...@@ -758,13 +758,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);
...@@ -831,7 +831,7 @@ namespace dynamicgraph ...@@ -831,7 +831,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);
...@@ -845,7 +845,7 @@ namespace dynamicgraph ...@@ -845,7 +845,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);
......
Supports Markdown
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