Commit 024ef4e8 authored by Paul Dandignac's avatar Paul Dandignac

Addition of a SE3 task for each hand

parent 90409a65
Pipeline #1513 passed with stage
in 21 minutes and 26 seconds
......@@ -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)
......
......@@ -52,6 +52,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 ***************************/
......@@ -406,7 +414,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)
{
......
This diff is collapsed.
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