Commit 779783b6 authored by Paul Dandignac's avatar Paul Dandignac
Browse files

Addition of a SE3 task for each hand

parent 809abf65
......@@ -170,6 +170,13 @@ namespace dynamicgraph {
void display(std::ostream & os) const;
};
struct HandUtil
{
std::string m_Left_Hand_Frame_Name;
std::string m_Right_Hand_Frame_Name;
void display(std::ostream & os) const;
};
struct RobotUtil
{
public:
......@@ -182,6 +189,9 @@ namespace dynamicgraph {
/// Foot information
FootUtil m_foot_util;
/// Hand information
HandUtil m_hand_util;
/// Map from the urdf index to the SoT index.
std::vector<Index> m_urdf_to_sot;
......
......@@ -131,13 +131,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 &);
......
......@@ -82,6 +82,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);
......@@ -93,6 +97,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);
......@@ -110,6 +120,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);
......@@ -117,6 +129,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);
......@@ -169,10 +182,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);
......@@ -207,9 +226,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;
......@@ -218,19 +257,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)
......
......@@ -53,6 +53,14 @@ namespace dynamicgraph
os << "Left Foot Frame Name:" << m_Left_Foot_Frame_Name << std::endl;
os << "Right Foot Frame Name:" << m_Right_Foot_Frame_Name << std::endl;
}
/******************** HandUtil ***************************/
void HandUtil::display(std::ostream &os) const
{
os << "Left Hand Frame Name:" << m_Left_Hand_Frame_Name << std::endl;
os << "Right Hand Frame Name:" << m_Right_Hand_Frame_Name << std::endl;
}
/******************** ForceUtil ***************************/
......@@ -407,7 +415,8 @@ namespace dynamicgraph
display(std::ostream &os) const
{
m_force_util.display(os);
m_foot_util.display(os);
m_foot_util.display(os);
m_hand_util.display(os);
os << "Nb of joints: " << m_nbJoints <<std::endl;
os << "Urdf file name: " << m_urdf_filename << std::endl;
......
......@@ -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)
{
......
......@@ -32,6 +32,33 @@
#include <boost/test/unit_test.hpp>
#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
......@@ -72,6 +99,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 \
......@@ -88,12 +121,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 \
......@@ -140,10 +176,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 \
<< m_dv_desSOUT \
......@@ -175,6 +217,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)
......@@ -191,12 +239,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)
......@@ -249,10 +300,16 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_OUT(base_orientation, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(left_foot_pos, 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_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, 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_OUT(right_foot_acc_des, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_INNER(active_joints_checked, dg::Vector, m_active_jointsSIN)
......@@ -262,8 +319,11 @@ namespace dynamicgraph
,m_firstTime(true)
,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();
......@@ -298,8 +358,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()
......@@ -355,6 +427,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)
{
......@@ -378,6 +486,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)
{
......@@ -403,6 +531,8 @@ namespace dynamicgraph
const Eigen::Vector3d& kd_com = m_kd_comSIN(0);
const Eigen::Vector6d& kp_feet = m_kp_feetSIN(0);
const Eigen::Vector6d& kd_feet = m_kd_feetSIN(0);
const Eigen::Vector6d& kp_hands = m_kp_handsSIN(0);
const Eigen::Vector6d& kd_hands = m_kd_handsSIN(0);
const VectorN& kp_posture = m_kp_postureSIN(0);
const VectorN& kd_posture = m_kd_postureSIN(0);
const VectorN& rotor_inertias_sot = m_rotor_inertiasSIN(0);
......@@ -413,6 +543,7 @@ namespace dynamicgraph
assert(rotor_inertias_sot.size()==m_robot_util->m_nbJoints);
assert(gear_ratios_sot.size()==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);
......@@ -489,13 +620,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());
......@@ -610,6 +753,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);
......@@ -642,6 +793,7 @@ namespace dynamicgraph
const VectorN& kd_pos = m_kd_posSIN(iter);
assert(kd_pos.size()==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);
......@@ -674,6 +826,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);
......@@ -707,6 +887,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);
......@@ -748,6 +935,7 @@ namespace dynamicgraph
m_timeLast = 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);
......@@ -1187,6 +1375,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)
......@@ -1202,6 +1406,21 @@ namespace dynamicgraph
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(right_hand_pos, dynamicgraph::Vector)