Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Update 11 : add interface of set footstep #767

Merged
merged 5 commits into from
Aug 25, 2015
Merged

Update 11 : add interface of set footstep #767

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
e0d18a3
[GaitGenerator.h] add default constructor of step_node
eisoku9618 Aug 24, 2015
92ce0a7
[AutoBalancer.cpp] do not print unless DEBUG mode
eisoku9618 Aug 24, 2015
a130183
[GaitGenerator.h] print index of foot steps
eisoku9618 Aug 24, 2015
0f19613
[GaitGenerator.h] use weight factor in get_swing_support_mid_coords f…
eisoku9618 Aug 24, 2015
c03d499
[idl/AutoBalancerService.idl, AutoBalancer.*, AutoBalancerService_imp…
eisoku9618 Aug 24, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
33 changes: 29 additions & 4 deletions idl/AutoBalancerService.idl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,15 @@ module OpenHRP
string leg;
};

/**
* @struct Footsteps
* @brief Foot step for multi legs.
*/
struct Footsteps
{
sequence<Footstep> fs;
};

/**
* @struct StepParam
* @brief Step parameter for one step
Expand All @@ -43,13 +52,29 @@ module OpenHRP
double heel_angle;
};

/**
* @struct StepParams
* @brief Step parameters for multi step
*/
struct StepParams
{
sequence<StepParam> sps;
};

/**
* @struct FootstepSequence
* @brief Sequence of foot step.
*/
typedef sequence<Footstep> FootstepSequence;
typedef sequence<StepParam> StepParamSequence;

/**
* @struct FootstepsSequence
* @brief Sequence of foot steps.
*/
typedef sequence<Footsteps> FootstepsSequence;
typedef sequence<StepParams> StepParamsSequence;

/**
* @enum SupportLegState
* @brief State of support leg.
Expand Down Expand Up @@ -218,19 +243,19 @@ module OpenHRP

/**
* @brief Set footsteps. Returns without waiting for whole steps to be executed.
* @param fs is sequence of FootStep structure.
* @param fss is sequence of FootStep structure.
overwrite_fs_idx is index to be overwritten. overwrite_fs_idx is used only in walking.
* @return true if set successfully, false otherwise
*/
boolean setFootSteps(in FootstepSequence fs, in long overwrite_fs_idx);
boolean setFootSteps(in FootstepsSequence fss, in long overwrite_fs_idx);

/**
* @brief Set footsteps. Returns without waiting for whole steps to be executed.
* @param fs is sequence of FootStepWithParam structure.
* @param fss is sequence of FootStepWithParam structure.
overwrite_fs_idx is index to be overwritten. overwrite_fs_idx is used only in walking.
* @return true if set successfully, false otherwise
*/
boolean setFootStepsWithParam(in FootstepSequence fs, in StepParamSequence sps, in long overwrite_fs_idx);
boolean setFootStepsWithParam(in FootstepsSequence fss, in StepParamsSequence spss, in long overwrite_fs_idx);

/**
* @brief Wait for whole footsteps are executed.
Expand Down
164 changes: 98 additions & 66 deletions rtc/AutoBalancer/AutoBalancer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -591,7 +591,7 @@ void AutoBalancer::getTargetParameters()
m_contactStates.data[contact_states_index_map["lleg"]] = true;
break;
default:
std::cerr << "not implemented yet " << std::endl;
if (DEBUGP) std::cerr << "not implemented yet " << std::endl;
break;
}
m_controlSwingSupportTime.data[contact_states_index_map["rleg"]] = gg->get_current_swing_time(0);
Expand Down Expand Up @@ -1012,84 +1012,116 @@ bool AutoBalancer::releaseEmergencyStop ()
return true;
}

bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs, CORBA::Long overwrite_fs_idx)
bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx)
{
OpenHRP::AutoBalancerService::StepParamSequence sps;
sps.length(fs.length());
OpenHRP::AutoBalancerService::StepParamsSequence spss;
spss.length(fss.length());
// If gg_is_walking is false, initial footstep will be double support. So, set 0 for step_height and toe heel angles.
// If gg_is_walking is true, do not set to 0.
for (size_t i = 0; i < sps.length(); i++) sps[i].step_height = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
for (size_t i = 0; i < sps.length(); i++) sps[i].step_time = gg->get_default_step_time();
for (size_t i = 0; i < sps.length(); i++) sps[i].toe_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_toe_angle());
for (size_t i = 0; i < sps.length(); i++) sps[i].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_heel_angle());
setFootStepsWithParam(fs, sps, overwrite_fs_idx);
for (size_t i = 0; i < spss.length(); i++) {
spss[i].sps.length(fss[i].fs.length());
for (size_t j = 0; j < spss[i].sps.length(); j++) {
spss[i].sps[j].step_height = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
spss[i].sps[j].step_time = gg->get_default_step_time();
spss[i].sps[j].toe_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
spss[i].sps[j].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
}
}
setFootStepsWithParam(fss, spss, overwrite_fs_idx);
}

bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps, CORBA::Long overwrite_fs_idx)
bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx)
{
if (!is_stop_mode) {
std::cerr << "[" << m_profile.instance_name << "] setFootSteps" << std::endl;
if (!is_stop_mode) {
std::cerr << "[" << m_profile.instance_name << "] setFootStepsList" << std::endl;

// Initial footstep Snapping
coordinates tmpfs, fstrans;
step_node initial_support_step, initial_input_step;
{
std::vector<step_node> initial_support_steps;
if (gg_is_walking) {
if (overwrite_fs_idx <= 0) {
std::cerr << "[" << m_profile.instance_name << "] Invalid overwrite index = " << overwrite_fs_idx << std::endl;
return false;
}
if (!gg->get_footstep_nodes_by_index(initial_support_steps, overwrite_fs_idx-1)) {
std::cerr << "[" << m_profile.instance_name << "] Invalid overwrite index = " << overwrite_fs_idx << std::endl;
return false;
}
} else {
// If walking, snap initial leg to current ABC foot coords.
for (size_t i = 0; i < fss[0].fs.length(); i++) {
initial_support_steps.push_back(step_node(std::string(fss[0].fs[i].leg), ikp[std::string(fss[0].fs[i].leg)].target_end_coords, 0, 0, 0, 0));
}
}
initial_support_step = initial_support_steps.front(); /* use only one leg for representation */
}
{
std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
for (size_t i = 0; i < fss[0].fs.length(); i++) {
if (std::string(fss[0].fs[i].leg) == leg_type_map[initial_support_step.l_r]) {
coordinates tmp;
memcpy(tmp.pos.data(), fss[0].fs[i].pos, sizeof(double)*3);
tmp.rot = (Eigen::Quaternion<double>(fss[0].fs[i].rot[0], fss[0].fs[i].rot[1], fss[0].fs[i].rot[2], fss[0].fs[i].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
initial_input_step = step_node(std::string(fss[0].fs[i].leg), tmp, 0, 0, 0, 0);
}
}
}

// Initial footstep Snapping
coordinates tmpfs, initial_support_coords, initial_input_coords, fstrans;
if (gg_is_walking) {
if (overwrite_fs_idx <= 0) {
std::cerr << "[" << m_profile.instance_name << "] Invalid overwrite index = " << overwrite_fs_idx << std::endl;
return false;
// Get footsteps
std::vector< std::vector<coordinates> > fs_vec_list;
std::vector< std::vector<std::string> > leg_name_vec_list;
for (size_t i = 0; i < fss.length(); i++) {
std::vector<coordinates> fs_vec;
std::vector<std::string> leg_name_vec;
for (size_t j = 0; j < fss[i].fs.length(); j++) {
std::string leg(fss[i].fs[j].leg);
if (std::find(leg_names.begin(), leg_names.end(), leg) != leg_names.end()) {
memcpy(tmpfs.pos.data(), fss[i].fs[j].pos, sizeof(double)*3);
tmpfs.rot = (Eigen::Quaternion<double>(fss[i].fs[j].rot[0], fss[i].fs[j].rot[1], fss[i].fs[j].rot[2], fss[i].fs[j].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
initial_input_step.worldcoords.transformation(fstrans, tmpfs);
tmpfs = initial_support_step.worldcoords;
tmpfs.transform(fstrans);
} else {
std::cerr << "[" << m_profile.instance_name << "] No such target : " << leg << std::endl;
return false;
}
leg_name_vec.push_back(leg);
fs_vec.push_back(tmpfs);
}
leg_name_vec_list.push_back(leg_name_vec);
fs_vec_list.push_back(fs_vec);
}
if (!gg->get_footstep_coords_by_index(initial_support_coords, overwrite_fs_idx-1)) {
std::cerr << "[" << m_profile.instance_name << "] Invalid overwrite index = " << overwrite_fs_idx << std::endl;
if (spss.length() != fs_vec_list.size()) {
std::cerr << "[" << m_profile.instance_name << "] StepParam length " << spss.length () << " != Footstep length " << fs_vec_list.size() << std::endl;
return false;
}
std::cerr << "[" << m_profile.instance_name << "] print footsteps " << std::endl;
std::vector< std::vector<step_node> > fnsl;
for (size_t i = 0; i < fs_vec_list.size(); i++) {
if (!(gg_is_walking && i == 0)) { // If initial footstep, e.g., not walking, pass user-defined footstep list. If walking, pass cdr footsteps in order to neglect initial double support leg.
std::vector<step_node> tmp_fns;
for (size_t j = 0; j < fs_vec_list.at(j).size(); j++) {
tmp_fns.push_back(step_node(leg_name_vec_list[i][j], fs_vec_list[i][j], spss[i].sps[j].step_height, spss[i].sps[j].step_time, spss[i].sps[j].toe_angle, spss[i].sps[j].heel_angle));
}
fnsl.push_back(tmp_fns);
}
}
if (gg_is_walking) {
std::cerr << "[" << m_profile.instance_name << "] Set overwrite footsteps" << std::endl;
gg->set_overwrite_foot_steps_list(fnsl);
gg->set_overwrite_foot_step_index(overwrite_fs_idx);
} else {
std::cerr << "[" << m_profile.instance_name << "] Set normal footsteps" << std::endl;
gg->set_foot_steps_list(fnsl);
startWalking();
}
return true;
} else {
// If walking, snap initial leg to current ABC foot coords.
initial_support_coords = ikp[std::string(fs[0].leg)].target_end_coords;
}
memcpy(initial_input_coords.pos.data(), fs[0].pos, sizeof(double)*3);
initial_input_coords.rot = (Eigen::Quaternion<double>(fs[0].rot[0], fs[0].rot[1], fs[0].rot[2], fs[0].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)

// Get footsteps
std::vector<coordinates> fs_vec;
std::vector<std::string> leg_name_vec;
for (size_t i = 0; i < fs.length(); i++) {
std::string leg(fs[i].leg);
if (leg == "rleg" || leg == "lleg") {
memcpy(tmpfs.pos.data(), fs[i].pos, sizeof(double)*3);
tmpfs.rot = (Eigen::Quaternion<double>(fs[i].rot[0], fs[i].rot[1], fs[i].rot[2], fs[i].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
initial_input_coords.transformation(fstrans, tmpfs);
tmpfs = initial_support_coords;
tmpfs.transform(fstrans);
leg_name_vec.push_back(leg);
fs_vec.push_back(tmpfs);
} else {
std::cerr << "[" << m_profile.instance_name << "] No such target : " << leg << std::endl;
return false;
}
}
if (sps.length() != fs_vec.size()) {
std::cerr << "[" << m_profile.instance_name << "] StepParam length " << sps.length () << " != Footstep length " << fs_vec.size() << std::endl;
std::cerr << "[" << m_profile.instance_name << "] Cannot setFootSteps while walking." << std::endl;
return false;
}
std::cerr << "[" << m_profile.instance_name << "] print footsteps " << std::endl;
std::vector< std::vector<step_node> > fnsl;
for (size_t i = 0; i < fs_vec.size(); i++) {
if (!(gg_is_walking && i == 0)) // If initial footstep, e.g., not walking, pass user-defined footstep list. If walking, pass cdr footsteps in order to neglect initial double support leg.
fnsl.push_back(boost::assign::list_of(step_node(leg_name_vec[i], fs_vec[i], sps[i].step_height, sps[i].step_time, sps[i].toe_angle, sps[i].heel_angle)));
}
if (gg_is_walking) {
std::cerr << "[" << m_profile.instance_name << "] Set overwrite footsteps" << std::endl;
gg->set_overwrite_foot_steps_list(fnsl);
gg->set_overwrite_foot_step_index(overwrite_fs_idx);
} else {
std::cerr << "[" << m_profile.instance_name << "] Set normal footsteps" << std::endl;
gg->set_foot_steps_list(fnsl);
startWalking();
}
return true;
} else {
std::cerr << "[" << m_profile.instance_name << "] Cannot setFootSteps while walking." << std::endl;
return false;
}
}

void AutoBalancer::waitFootSteps()
Expand Down
2 changes: 2 additions & 0 deletions rtc/AutoBalancer/AutoBalancer.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,9 @@ class AutoBalancer
bool goStop();
bool emergencyStop ();
bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs, CORBA::Long overwrite_fs_idx);
bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx);
bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps, CORBA::Long overwrite_fs_idx);
bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx);
void waitFootSteps();
void waitFootStepsEarly(const double tm);
bool startAutoBalancer(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);
Expand Down
8 changes: 4 additions & 4 deletions rtc/AutoBalancer/AutoBalancerService_impl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,14 +29,14 @@ CORBA::Boolean AutoBalancerService_impl::emergencyStop()
return m_autobalancer->emergencyStop();
};

CORBA::Boolean AutoBalancerService_impl::setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs, CORBA::Long overwrite_fs_idx)
CORBA::Boolean AutoBalancerService_impl::setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx)
{
return m_autobalancer->setFootSteps(fs, overwrite_fs_idx);
return m_autobalancer->setFootSteps(fss, overwrite_fs_idx);
}

CORBA::Boolean AutoBalancerService_impl::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps, CORBA::Long overwrite_fs_idx)
CORBA::Boolean AutoBalancerService_impl::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx)
{
return m_autobalancer->setFootStepsWithParam(fs, sps, overwrite_fs_idx);
return m_autobalancer->setFootStepsWithParam(fss, spss, overwrite_fs_idx);
}

void AutoBalancerService_impl::waitFootSteps()
Expand Down
4 changes: 2 additions & 2 deletions rtc/AutoBalancer/AutoBalancerService_impl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,8 @@ class AutoBalancerService_impl
CORBA::Boolean goVelocity( CORBA::Double vx, CORBA::Double vy, CORBA::Double vth);
CORBA::Boolean goStop();
CORBA::Boolean emergencyStop();
CORBA::Boolean setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs, CORBA::Long overwrite_fs_idx);
CORBA::Boolean setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps, CORBA::Long overwrite_fs_idx);
CORBA::Boolean setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx);
CORBA::Boolean setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx);
void waitFootSteps();
void waitFootStepsEarly(CORBA::Double tm);
CORBA::Boolean startAutoBalancer(const OpenHRP::AutoBalancerService::StrSequence& limbs);
Expand Down
10 changes: 7 additions & 3 deletions rtc/AutoBalancer/GaitGenerator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,9 @@ namespace rats
leg_type l_r;
coordinates worldcoords;
double step_height, step_time, toe_angle, heel_angle;
step_node () : l_r(RLEG), worldcoords(coordinates()),
step_height(), step_time(),
toe_angle(), heel_angle(){};
step_node (const leg_type _l_r, const coordinates& _worldcoords,
const double _step_height, const double _step_time,
const double _toe_angle, const double _heel_angle)
Expand Down Expand Up @@ -700,7 +703,7 @@ namespace rats
coordinates tmp_swg_mid, tmp_sup_mid;
multi_mid_coords(tmp_swg_mid, swg_coords);
multi_mid_coords(tmp_sup_mid, sup_coords);
mid_coords(ret, 0.5, tmp_swg_mid, tmp_sup_mid);
mid_coords(ret, static_cast<double>(sup_coords.size()) / (swg_coords.size() + sup_coords.size()), tmp_swg_mid, tmp_sup_mid);
};
std::vector<leg_type> get_current_support_states () const
{
Expand Down Expand Up @@ -944,10 +947,10 @@ namespace rats
return false;
}
};
bool get_footstep_coords_by_index (coordinates& cs, const size_t idx)
bool get_footstep_nodes_by_index (std::vector<step_node>& csl, const size_t idx)
{
if (footstep_nodes_list.size()-1 >= idx) {
cs = footstep_nodes_list[idx].front().worldcoords;
csl = footstep_nodes_list.at(idx);
return true;
} else {
return false;
Expand All @@ -956,6 +959,7 @@ namespace rats
void print_footstep_nodes_list (const std::vector< std::vector<step_node> > _footstep_nodes_list) const
{
for (size_t i = 0; i < _footstep_nodes_list.size(); i++) {
std::cerr << "foot step index : " << i << std::endl;
for (size_t j = 0; j < _footstep_nodes_list.at(i).size(); j++) {
std::cerr << _footstep_nodes_list.at(i).at(j) << std::endl;
}
Expand Down
Loading