Commit bab0e4c2 authored by ggory15's avatar ggory15
Browse files

update viapoint ftn

parent cbc5ad9d
......@@ -38,6 +38,7 @@ namespace timeopt{
.def("setPhase",&Problem::setPhase, (bp::arg("index"), bp::arg("phase")),"Set ith phase.")
.def("setMass",&Problem::setMass, (bp::args("mass")),"Set robot mass.")
.def("setFinalCOM",&Problem::setFinalCOM, (bp::arg("COM")),"Set Final COM (Vector3d).")
.def("setViapoint",&Problem::setViapoint, (bp::args("time", "viapoint(Vector3d)")), "Set Viapoint (time, Vector3d).")
.def("resize",&Problem::resize, (bp::args("size")),"Resize phases size.")
.def("getTrajectorySize", &Problem::getNumSize, "get Trajectory length")
......
......@@ -24,21 +24,7 @@ using namespace std;
typedef Eigen::Matrix<double, 8, 1> Vector8d;
public:
InitialState()
: ini_com_(Eigen::Vector3d(.0, .0, .0))
, amom_(Eigen::Vector3d(.0, .0, .0))
, lmom_(Eigen::Vector3d(.0, .0, .0))
, eef_frc_rf_(Eigen::Vector3d(.0, .0, .5))
, eef_frc_lf_(Eigen::Vector3d(.0, .0, .5))
, eef_frc_rh_(Eigen::Vector3d(.0, .0, .0))
, eef_frc_lh_(Eigen::Vector3d(.0, .0, .0))
{
eef_rf.setZero();
eef_lf.setZero();
eef_rh.setZero();
eef_lh.setZero();
};
InitialState();
~InitialState(){};
inline void setCOM(const Eigen::Vector3d& com) {
......@@ -61,7 +47,10 @@ using namespace std;
private:
Eigen::Vector3d ini_com_, amom_, lmom_;
Eigen::Vector3d eef_frc_rf_, eef_frc_lf_, eef_frc_lh_, eef_frc_rh_;
Eigen::Vector3d eef_frc_rf_;
Eigen::Vector3d eef_frc_lf_;
Eigen::Vector3d eef_frc_lh_;
Eigen::Vector3d eef_frc_rh_;
Vector8d eef_rf, eef_lf, eef_rh, eef_lh;
EndeffectorID ee_id;
YAML::Node init_cfg;
......@@ -112,13 +101,11 @@ using namespace std;
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef Eigen::Matrix<double, 4, 1> Viapoint;
typedef std::vector<Viapoint,Eigen::aligned_allocator<Viapoint>> ViapointVector;
public:
ContactPlanner()
: time_horizon_(0.0)
, robot_mass_(0.0)
, com_displacement_(Eigen::Vector3d(.0, .0, .0))
{};
ContactPlanner();
~ContactPlanner(){};
void initialize(const std::string cfg_path, const InitialState& init_state, const ContactState& contact_state);
......@@ -132,11 +119,19 @@ using namespace std;
inline void setFinalCOM(const Eigen::Vector3d & end_com){
end_com_ = end_com;
}
inline void setViapoint(const double& time, const Eigen::Vector3d & via){
Viapoint v;
v << time, via(0), via(1), via(2);
viapoints_.push_back(v);
}
void saveToFile();
private:
double robot_mass_, time_horizon_;
Eigen::Vector3d com_displacement_, end_com_;
unsigned int num_com_viapoints_;
ViapointVector viapoints_;
std::string file_location_;
momentumopt::PlannerSetting planner_setting_;
......
......@@ -56,6 +56,9 @@
final_com_ = com;
planner_->setFinalCOM(com);
}
void setViapoint(const double& time, const Vector3d& point){
planner_->setViapoint(time, point);
}
void resize(const Index index){
phases_.resize(index);
contact_state_->resize(index);
......
......@@ -2,6 +2,20 @@
namespace timeopt
{
InitialState::InitialState()
: ini_com_(Eigen::Vector3d(.0, .0, .0))
, amom_(Eigen::Vector3d(.0, .0, .0))
, lmom_(Eigen::Vector3d(.0, .0, .0))
, eef_frc_rf_(Eigen::Vector3d(.0, .0, .5))
, eef_frc_lf_(Eigen::Vector3d(.0, .0, .5))
, eef_frc_rh_(Eigen::Vector3d(.0, .0, .0))
, eef_frc_lh_(Eigen::Vector3d(.0, .0, .0))
{
eef_rf.setZero();
eef_lf.setZero();
eef_rh.setZero();
eef_lh.setZero();
}
void InitialState::setEEForceRatio(const Eigen::Vector3d& ratio, EndeffectorID ee){
switch (ee){
case RF:
......@@ -148,6 +162,11 @@ namespace timeopt
file_out << contact_cfg;
}
ContactPlanner::ContactPlanner()
: time_horizon_(0.0)
, robot_mass_(0.0)
, com_displacement_(Eigen::Vector3d(.0, .0, .0))
{}
void ContactPlanner::initialize(const std::string cfg_path, const InitialState& init_state, const ContactState& contact_state){
planner_setting_.initialize(cfg_path);
file_location_ = cfg_path;
......@@ -170,6 +189,13 @@ namespace timeopt
com_displacement_ = end_com_ - init_com;
cfg_pars["planner_variables"]["com_displacement"] = com_displacement_;
cfg_pars["planner_variables"]["com_displacement"].SetStyle(YAML::EmitterStyle::Flow);
cfg_pars["planner_variables"]["num_com_viapoints"] = viapoints_.size();
for (size_t i=0; i<viapoints_.size(); i++){
cfg_pars["planner_variables"]["com_viapoints"]["via"+std::to_string(i)] = viapoints_[i];
cfg_pars["planner_variables"]["com_viapoints"]["via"+std::to_string(i)].SetStyle(YAML::EmitterStyle::Flow);
}
std::ofstream file_out(file_location_.substr(0, file_location_.size()-5)+"_final.yaml");
file_out << init_cfg << endl << endl << contact_cfg << endl << endl << cfg_pars;
}
......
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