Commit b4979274 authored by Paul Dandignac's avatar Paul Dandignac Committed by Noëlie Ramuzat
Browse files

Rename different task states

parent 3f0fec5c
build
_build*
*~
\.travis
cmake
Subproject commit dc8b946d456d2c41ad12b819111b005148c68031
Subproject commit 1d9aeca25e970d2d967fd5be0fb93fe961db121b
......@@ -132,13 +132,17 @@ namespace dynamicgraph {
const dynamicgraph::Vector &uq);
void setForceNameToForceId(const std::string& forceName,
const double & forceId);
/// Commands related to FootUtil
void setRightFootSoleXYZ(const dynamicgraph::Vector &);
/// Commands related to FootUtil
void setRightFootSoleXYZ(const dynamicgraph::Vector &);
void setRightFootForceSensorXYZ(const dynamicgraph::Vector &);
void setFootFrameName(const std::string &, const std::string &);
void setFootFrameName(const std::string &, const std::string &);
void setImuJointName(const std::string &);
void displayRobotUtil();
void displayRobotUtil();
/// Commands related to HandUtil
void setHandFrameName(const std::string &, const std::string &);
/// Set the mapping between urdf and sot.
void setJoints(const dynamicgraph::Vector &);
......
......@@ -85,6 +85,10 @@ namespace dynamicgraph {
void removeLeftFootContact(const double& transitionTime);
void addRightFootContact(const double& transitionTime);
void addLeftFootContact(const double& transitionTime);
void addTaskRightHand(/*const double& transitionTime*/);
void removeTaskRightHand(const double& transitionTime);
void addTaskLeftHand(/*const double& transitionTime*/);
void removeTaskLeftHand(const double& transitionTime);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector);
......@@ -96,6 +100,12 @@ namespace dynamicgraph {
DECLARE_SIGNAL_IN(lf_ref_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(lf_ref_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(lf_ref_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(rh_ref_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(rh_ref_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(rh_ref_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(lh_ref_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(lh_ref_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(lh_ref_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector);
......@@ -113,6 +123,8 @@ namespace dynamicgraph {
DECLARE_SIGNAL_IN(kd_com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_feet, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_feet, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_hands, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_hands, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_posture, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector);
......@@ -120,6 +132,7 @@ namespace dynamicgraph {
DECLARE_SIGNAL_IN(w_com, double);
DECLARE_SIGNAL_IN(w_feet, double);
DECLARE_SIGNAL_IN(w_hands, double);
DECLARE_SIGNAL_IN(w_posture, double);
DECLARE_SIGNAL_IN(w_base_orientation, double);
DECLARE_SIGNAL_IN(w_torques, double);
......@@ -172,10 +185,16 @@ namespace dynamicgraph {
DECLARE_SIGNAL_OUT(base_orientation, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_foot_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_foot_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_hand_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_hand_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_foot_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_foot_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_hand_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_hand_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_foot_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_foot_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_hand_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_hand_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_foot_acc_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_foot_acc_des, dynamicgraph::Vector);
......@@ -210,9 +229,29 @@ namespace dynamicgraph {
ContactState m_contactState;
double m_contactTransitionTime; /// end time of the current contact transition (if any)
enum RightHandState
{
HIGH_RIGHT_HAND = 0,
/*HIGH_RIGHT_HAND_TRANSITION = 1,*/
LOW_RIGHT_HAND = 1
};
RightHandState m_rightHandState;
enum LeftHandState
{
HIGH_LEFT_HAND = 0,
/*HIGH_LEFT_HAND_TRANSITION = 1,*/
LOW_LEFT_HAND = 1
};
LeftHandState m_leftHandState;
/*double m_handsTransitionTime;*/ /// end time of the current transition (if any)
int m_frame_id_rf; /// frame id of right foot
int m_frame_id_lf; /// frame id of left foot
int m_frame_id_rh; /// frame id of right hand
int m_frame_id_lh; /// frame id of left hand
/// tsid
tsid::robots::RobotWrapper * m_robot;
tsid::solvers::SolverHQPBase * m_hqpSolver;
......@@ -221,19 +260,26 @@ namespace dynamicgraph {
tsid::InverseDynamicsFormulationAccForce * m_invDyn;
tsid::contacts::Contact6d * m_contactRF;
tsid::contacts::Contact6d * m_contactLF;
tsid::contacts::Contact6d * m_contactRH;
tsid::contacts::Contact6d * m_contactLH;
tsid::tasks::TaskComEquality * m_taskCom;
tsid::tasks::TaskSE3Equality * m_taskRF;
tsid::tasks::TaskSE3Equality * m_taskLF;
tsid::tasks::TaskSE3Equality * m_taskRH;
tsid::tasks::TaskSE3Equality * m_taskLH;
tsid::tasks::TaskJointPosture * m_taskPosture;
tsid::tasks::TaskJointPosture * m_taskBlockedJoints;
tsid::trajectories::TrajectorySample m_sampleCom;
tsid::trajectories::TrajectorySample m_sampleRF;
tsid::trajectories::TrajectorySample m_sampleLF;
tsid::trajectories::TrajectorySample m_sampleRH;
tsid::trajectories::TrajectorySample m_sampleLH;
tsid::trajectories::TrajectorySample m_samplePosture;
double m_w_com;
double m_w_posture;
double m_w_hands;
tsid::math::Vector m_dv_sot; /// desired accelerations (sot order)
tsid::math::Vector m_dv_urdf; /// desired accelerations (urdf order)
......
......@@ -156,6 +156,11 @@ namespace dynamicgraph
docCommandVoid2("Set the Frame Name for the Foot Name.",
"(string) Foot name",
"(string) Frame name")));
addCommand("setHandFrameName",
makeCommandVoid2(*this, &ControlManager::setHandFrameName,
docCommandVoid2("Set the Frame Name for the Hand Name.",
"(string) Hand name",
"(string) Frame name")));
addCommand("setImuJointName",
makeCommandVoid1(*this, &ControlManager::setImuJointName,
docCommandVoid1("Set the Joint Name wich IMU is attached to.",
......@@ -569,7 +574,7 @@ namespace dynamicgraph
return;
}
m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ = xyz;
m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ = xyz;
}
void ControlManager::setRightFootForceSensorXYZ(const dynamicgraph::Vector &xyz)
......@@ -598,6 +603,22 @@ namespace dynamicgraph
else
SEND_WARNING_STREAM_MSG("Did not understand the foot name !" + FootName);
}
void ControlManager::setHandFrameName( const std::string &HandName,
const std::string &FrameName)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot set hand frame name!");
return;
}
if (HandName=="Left")
m_robot_util->m_hand_util.m_Left_Hand_Frame_Name = FrameName;
else if (HandName=="Right")
m_robot_util->m_hand_util.m_Right_Hand_Frame_Name = FrameName;
else
SEND_WARNING_STREAM_MSG("Did not understand the hand name !" + HandName);
}
void ControlManager::setImuJointName(const std::string &JointName)
{
......
......@@ -33,6 +33,33 @@
#include <sot/torque_control/commands-helper.hh>
#include <sot/torque_control/inverse-dynamics-balance-controller.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-sot-torqe-control.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 sot
......@@ -74,6 +101,12 @@ namespace dynamicgraph
<< m_lf_ref_posSIN \
<< m_lf_ref_velSIN \
<< m_lf_ref_accSIN \
<< m_rh_ref_posSIN \
<< m_rh_ref_velSIN \
<< m_rh_ref_accSIN \
<< m_lh_ref_posSIN \
<< m_lh_ref_velSIN \
<< m_lh_ref_accSIN \
<< m_posture_ref_posSIN \
<< m_posture_ref_velSIN \
<< m_posture_ref_accSIN \
......@@ -90,12 +123,15 @@ namespace dynamicgraph
<< m_kd_comSIN \
<< m_kp_feetSIN \
<< m_kd_feetSIN \
<< m_kp_handsSIN \
<< m_kd_handsSIN \
<< m_kp_postureSIN \
<< m_kd_postureSIN \
<< m_kp_posSIN \
<< m_kd_posSIN \
<< m_w_comSIN \
<< m_w_feetSIN \
<< m_w_handsSIN \
<< m_w_postureSIN \
<< m_w_base_orientationSIN \
<< m_w_torquesSIN \
......@@ -144,10 +180,16 @@ namespace dynamicgraph
<< m_base_orientationSOUT \
<< m_right_foot_posSOUT \
<< m_left_foot_posSOUT \
<< m_right_hand_posSOUT \
<< m_left_hand_posSOUT \
<< m_right_foot_velSOUT \
<< m_left_foot_velSOUT \
<< m_right_hand_velSOUT \
<< m_left_hand_velSOUT \
<< m_right_foot_accSOUT \
<< m_left_foot_accSOUT \
<< m_right_hand_accSOUT \
<< m_left_hand_accSOUT \
<< m_right_foot_acc_desSOUT \
<< m_left_foot_acc_desSOUT
......@@ -177,6 +219,12 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_IN(lf_ref_pos, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(lf_ref_vel, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(lf_ref_acc, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(rh_ref_pos, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(rh_ref_vel, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(rh_ref_acc, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(lh_ref_pos, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(lh_ref_vel, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(lh_ref_acc, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector)
......@@ -193,12 +241,15 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_IN(kd_com, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(kp_feet, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(kd_feet, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(kp_hands, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(kd_hands, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(kp_posture, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(kd_posture, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(kp_pos, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(kd_pos, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(w_com, double)
,CONSTRUCT_SIGNAL_IN(w_feet, double)
,CONSTRUCT_SIGNAL_IN(w_hands, double)
,CONSTRUCT_SIGNAL_IN(w_posture, double)
,CONSTRUCT_SIGNAL_IN(w_base_orientation, double)
,CONSTRUCT_SIGNAL_IN(w_torques, double)
......@@ -251,11 +302,17 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_OUT(base_orientation, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(right_foot_pos, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(left_foot_pos, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(right_foot_vel, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(right_foot_pos, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(left_hand_pos, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(right_hand_pos, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(left_foot_vel, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(right_foot_acc, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(right_foot_vel, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(left_hand_vel, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(right_hand_vel, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(left_foot_acc, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(right_foot_acc_des, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(right_foot_acc, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(left_hand_acc, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(right_hand_acc, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(left_foot_acc_des, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_INNER(active_joints_checked, dg::Vector, m_active_jointsSIN)
,m_t(0.0)
......@@ -264,8 +321,12 @@ namespace dynamicgraph
,m_firstTime(true)
,m_contactState(DOUBLE_SUPPORT)
,m_timeLast(0)
,m_contactState(DOUBLE_SUPPORT)
,m_rightHandState(LOW_RIGHT_HAND)
,m_leftHandState(LOW_LEFT_HAND)
,m_robot_util(RefVoidRobotUtil())
{
RESETDEBUG5();
Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
m_zmp_des_RF.setZero();
......@@ -300,8 +361,20 @@ namespace dynamicgraph
makeCommandVoid1(*this, &InverseDynamicsBalanceController::removeLeftFootContact,
docCommandVoid1("Remove the contact at the left foot.",
"Transition time in seconds (double)")));
addCommand("addTaskRightHand",
makeCommandVoid0(*this, &InverseDynamicsBalanceController::addTaskRightHand,
docCommandVoid0("Adds an SE3 task for the right hand.")));
addCommand("removeTaskRightHand",
makeCommandVoid1(*this, &InverseDynamicsBalanceController::removeTaskRightHand,
docCommandVoid1("Removes the SE3 task for the right hand.",
"Transition time in seconds (double)")));
addCommand("addTaskLeftHand",
makeCommandVoid0(*this, &InverseDynamicsBalanceController::addTaskLeftHand,
docCommandVoid0("Raises the left hand.")));
addCommand("removeTaskLeftHand",
makeCommandVoid1(*this, &InverseDynamicsBalanceController::removeTaskLeftHand,
docCommandVoid1("lowers the left hand.",
"Transition time in seconds (double)")));
}
void InverseDynamicsBalanceController::updateComOffset()
......@@ -357,6 +430,42 @@ namespace dynamicgraph
m_contactState = RIGHT_SUPPORT;
}
}
void InverseDynamicsBalanceController::addTaskRightHand(/*const double& transitionTime*/)
{
if(m_rightHandState == LOW_RIGHT_HAND)
{
SEND_MSG("Adds right hand SE3 task in "/*+toString(transitionTime)+" s"*/, MSG_TYPE_INFO);
const double & w_hands = m_w_handsSIN.accessCopy();
m_invDyn->addMotionTask(*m_taskRH, w_hands, 1);
}
/*if(transitionTime>m_dt)
{
m_rightHandState = HIGH_RIGHT_HAND_TRANSITION;
m_handsTransitionTime = m_t + transitionTime;
}
else
m_rightHandState = HIGH_RIGHT_HAND;*/
m_rightHandState = HIGH_RIGHT_HAND;
}
void InverseDynamicsBalanceController::addTaskLeftHand(/*const double& transitionTime*/)
{
if(m_leftHandState == LOW_LEFT_HAND)
{
SEND_MSG("Raise left hand in "/*+toString(transitionTime)+" s"*/, MSG_TYPE_INFO);
const double & w_hands = m_w_handsSIN.accessCopy();
m_invDyn->addMotionTask(*m_taskLH, w_hands, 1);
}
/*if(transitionTime>m_dt)
{
m_leftHandState = HIGH_LEFT_HAND_TRANSITION;
m_handsTransitionTime = m_t + transitionTime;
}
else
m_leftHandState = HIGH_LEFT_HAND;*/
m_leftHandState = HIGH_LEFT_HAND;
}
void InverseDynamicsBalanceController::addRightFootContact(const double& transitionTime)
{
......@@ -382,6 +491,26 @@ namespace dynamicgraph
}
}
void InverseDynamicsBalanceController::removeTaskRightHand(const double& transitionTime)
{
if(m_rightHandState == HIGH_RIGHT_HAND)
{
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;
}
}
void InverseDynamicsBalanceController::removeTaskLeftHand(const double& transitionTime)
{
if(m_leftHandState == HIGH_LEFT_HAND)
{
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;
}
}
void InverseDynamicsBalanceController::init(const double& dt,
const std::string& robotRef)
{
......@@ -405,6 +534,8 @@ namespace dynamicgraph
const dg::sot::Vector6d& kd_contact = m_kd_constraintsSIN(0);
const Eigen::Vector3d& kp_com = m_kp_comSIN(0);
const Eigen::Vector3d& kd_com = m_kd_comSIN(0);
const dg::sot::Vector6d& kd_hands = m_kd_handsSIN(0);
const dg::sot::Vector6d& kp_hands = m_kp_handsSIN(0);
const dg::sot::Vector6d& kp_feet = m_kp_feetSIN(0);
const dg::sot::Vector6d& kd_feet = m_kd_feetSIN(0);
const VectorN& kp_posture = m_kp_postureSIN(0);
......@@ -417,6 +548,7 @@ namespace dynamicgraph
assert(rotor_inertias_sot.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
assert(gear_ratios_sot.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
m_w_hands = m_w_handsSIN(0);
m_w_com = m_w_comSIN(0);
m_w_posture = m_w_postureSIN(0);
const double & w_forces = m_w_forcesSIN(0);
......@@ -493,13 +625,25 @@ namespace dynamicgraph
m_taskPosture->Kp(kp_posture);
m_taskPosture->Kd(kd_posture);
m_invDyn->addMotionTask(*m_taskPosture, m_w_posture, 1);
m_taskRH = new TaskSE3Equality("task-rh", *m_robot, m_robot_util->m_hand_util.m_Right_Hand_Frame_Name);
m_taskRH->Kp(kp_hands);
m_taskRH->Kd(kd_hands);
m_taskLH = new TaskSE3Equality("task-lh", *m_robot, m_robot_util->m_hand_util.m_Left_Hand_Frame_Name);
m_taskLH->Kp(kp_hands);
m_taskLH->Kd(kd_hands);
m_sampleCom = TrajectorySample(3);
m_samplePosture = TrajectorySample(m_robot->nv()-6);
m_sampleRH = TrajectorySample(3);
m_frame_id_rf = (int)m_robot->model().getFrameId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name);
m_frame_id_lf = (int)m_robot->model().getFrameId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
m_frame_id_rh = (int)m_robot->model().getFrameId(m_robot_util->m_hand_util.m_Right_Hand_Frame_Name);
m_frame_id_lh = (int)m_robot->model().getFrameId(m_robot_util->m_hand_util.m_Left_Hand_Frame_Name);
m_hqpSolver = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST,
"eiquadprog-fast");
m_hqpSolver->resize(m_invDyn->nVar(), m_invDyn->nEq(), m_invDyn->nIn());
......@@ -614,6 +758,14 @@ namespace dynamicgraph
{
m_contactState = LEFT_SUPPORT;
}
/*if(m_rightHandState == HIGH_RIGHT_HAND_TRANSITION && m_t >= m_handsTransitionTime)
{
m_rightHandState = HIGH_RIGHT_HAND;
}
if(m_leftHandState == HIGH_LEFT_HAND_TRANSITION && m_t >= m_handsTransitionTime)
{
m_leftHandState = HIGH_LEFT_HAND;
}*/
getProfiler().start(PROFILE_READ_INPUT_SIGNALS);
m_w_feetSIN(iter);
......@@ -646,6 +798,7 @@ namespace dynamicgraph
const VectorN& kd_pos = m_kd_posSIN(iter);
assert(kd_pos.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
/*const double & w_hands = m_w_handsSIN(iter);*/
const double & w_com = m_w_comSIN(iter);
const double & w_posture = m_w_postureSIN(iter);
const double & w_forces = m_w_forcesSIN(iter);
......@@ -678,6 +831,34 @@ namespace dynamicgraph
m_taskLF->Kp(kp_feet);
m_taskLF->Kd(kd_feet);
}
if(m_rightHandState == HIGH_RIGHT_HAND /*|| m_rightHandState == HIGH_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);
const Vector6& ddx_rh_ref = m_rh_ref_accSIN(iter);
const Vector6& kp_hands = m_kp_handsSIN(iter);
const Vector6& kd_hands = m_kd_handsSIN(iter);
m_sampleRH.pos = x_rh_ref;
m_sampleRH.vel = dx_rh_ref;
m_sampleRH.acc = ddx_rh_ref;
m_taskRH->setReference(m_sampleRH);
m_taskRH->Kp(kp_hands);
m_taskRH->Kd(kd_hands);
}
if(m_leftHandState == HIGH_LEFT_HAND /*|| m_leftHandState == HIGH_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);
const Vector6& ddx_lh_ref = m_lh_ref_accSIN(iter);
const Vector6& kp_hands = m_kp_handsSIN(iter);
const Vector6& kd_hands = m_kd_handsSIN(iter);
m_sampleLH.pos = x_lh_ref;
m_sampleLH.vel = dx_lh_ref;
m_sampleLH.acc = ddx_lh_ref;
m_taskLH->setReference(m_sampleLH);
m_taskLH->Kp(kp_hands);
m_taskLH->Kd(kd_hands);
}
getProfiler().stop(PROFILE_READ_INPUT_SIGNALS);
......@@ -711,6 +892,13 @@ namespace dynamicgraph
m_invDyn->updateTaskWeight(m_taskPosture->name(), w_posture);
}
/*m_sampleRH.pos = x_rh_ref;
m_sampleRH.vel = dx_rh_ref;
m_sampleRH.acc = ddx_rh_ref;
m_taskRH->setReference(m_sampleRH);
m_taskRH->Kp(kp_hands);
m_taskRH->Kd(kd_hands);*/
const double & fMin = m_f_minSIN(0);
const double & fMaxRF = m_f_max_right_footSIN(iter);
const double & fMaxLF = m_f_max_left_footSIN(iter);
......@@ -752,6 +940,7 @@ namespace dynamicgraph
m_timeLast = static_cast<unsigned int>(iter);
const HQPData & hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf);
getProfiler().stop(PROFILE_PREPARE_INV_DYN);
getProfiler().start(PROFILE_HQP_SOLUTION);
......@@ -1201,6 +1390,22 @@ namespace dynamicgraph
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(left_hand_pos, dynamicgraph::Vector)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal left hand_pos before initialization!");
return s;
}
m_tau_desSOUT(iter);
se3::SE3 oMi;
s.resize(12);
m_robot->framePosition(m_invDyn->data(), m_frame_id_lh, oMi);
tsid::math::SE3ToVector(oMi, s);
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(right_foot_pos, dynamicgraph::Vector)
{
if(!m_initSucceeded)
......@@ -1218,6 +1423,21 @@ namespace dynamicgraph
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(right_hand_pos, dynamicgraph::Vector)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal right_hand_pos before initialization!");
return s;
}
m_tau_desSOUT(iter);
se3::SE3 oMi;
s.resize(12);
m_robot->framePosition(m_invDyn->data(), m_frame_id_rh, oMi);
tsid::math::SE3ToVector(oMi, s);
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(left_foot_vel, dynamicgraph::Vector)
{
if(!m_initSucceeded)
......@@ -1233,6 +1453,19 @@ namespace dynamicgraph
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(left_hand_vel, dynamicgraph::Vector)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal left_hand_vel before initialization!");
return s;
}
se3::Motion v;