Skip to content

Commit

Permalink
I will squash this commit: Update variable names following the naming…
Browse files Browse the repository at this point in the history 
… rule
  • Loading branch information
@eisoku9618
eisoku9618 committed Jul 23, 2015
1 parent 9415115 commit 435c1aa
Show file tree
Hide file tree
Showing 4 changed files with 451 additions and 451 deletions.
76 changes: 38 additions & 38 deletions rtc/AutoBalancer/AutoBalancer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -556,28 +556,28 @@ void AutoBalancer::getTargetParameters()
gg_solved = gg->proc_one_tick();
// for support leg
coordinates sp_coords, sw_coords, tmpc;
for (size_t i = 0; i < gg->get_support_leg_coords_list().size(); i++) {
sp_coords = coordinates(gg->get_support_leg_coords_list()[i].pos,
gg->get_support_leg_coords_list()[i].rot);
coordinates(ikp[gg->get_support_leg_list()[i]].localPos,
ikp[gg->get_support_leg_list()[i]].localR).inverse_transformation(tmpc);
for (size_t i = 0; i < gg->get_support_legs_coords().size(); i++) {
sp_coords = coordinates(gg->get_support_legs_coords()[i].pos,
gg->get_support_legs_coords()[i].rot);
coordinates(ikp[gg->get_support_legs()[i]].localPos,
ikp[gg->get_support_legs()[i]].localR).inverse_transformation(tmpc);
sp_coords.transform(tmpc);
ikp[gg->get_support_leg_list()[i]].target_p0 = sp_coords.pos;
ikp[gg->get_support_leg_list()[i]].target_r0 = sp_coords.rot;
ikp[gg->get_support_legs()[i]].target_p0 = sp_coords.pos;
ikp[gg->get_support_legs()[i]].target_r0 = sp_coords.rot;
}
// for swing leg
for (size_t i = 0; i < gg->get_swing_leg_coords_list().size(); i++) {
sw_coords = coordinates(gg->get_swing_leg_coords_list()[i].pos,
gg->get_swing_leg_coords_list()[i].rot);
coordinates(ikp[gg->get_swing_leg_list()[i]].localPos,
ikp[gg->get_swing_leg_list()[i]].localR).inverse_transformation(tmpc);
for (size_t i = 0; i < gg->get_swing_legs_coords().size(); i++) {
sw_coords = coordinates(gg->get_swing_legs_coords()[i].pos,
gg->get_swing_legs_coords()[i].rot);
coordinates(ikp[gg->get_swing_legs()[i]].localPos,
ikp[gg->get_swing_legs()[i]].localR).inverse_transformation(tmpc);
sw_coords.transform(tmpc);
ikp[gg->get_swing_leg_list()[i]].target_p0 = sw_coords.pos;
ikp[gg->get_swing_leg_list()[i]].target_r0 = sw_coords.rot;
ikp[gg->get_swing_legs()[i]].target_p0 = sw_coords.pos;
ikp[gg->get_swing_legs()[i]].target_r0 = sw_coords.rot;
}
gg->get_swing_support_mid_coords(tmp_fix_coords);
// TODO : assume biped
switch (gg->get_current_support_state_list().front()) {
switch (gg->get_current_support_states().front()) {
case BOTH:
m_contactStates.data[contact_states_index_map["rleg"]] = true;
m_contactStates.data[contact_states_index_map["lleg"]] = true;
Expand All @@ -596,12 +596,12 @@ void AutoBalancer::getTargetParameters()
}
m_controlSwingSupportTime.data[contact_states_index_map["rleg"]] = gg->get_current_swing_time(0);
m_controlSwingSupportTime.data[contact_states_index_map["lleg"]] = gg->get_current_swing_time(1);
m_limbCOPOffset[contact_states_index_map[gg->get_swing_leg_list().front()]].data.x = gg->get_swing_foot_zmp_offset_list().front()(0);
m_limbCOPOffset[contact_states_index_map[gg->get_swing_leg_list().front()]].data.y = gg->get_swing_foot_zmp_offset_list().front()(1);
m_limbCOPOffset[contact_states_index_map[gg->get_swing_leg_list().front()]].data.z = gg->get_swing_foot_zmp_offset_list().front()(2);
m_limbCOPOffset[contact_states_index_map[gg->get_support_leg_list().front()]].data.x = gg->get_support_foot_zmp_offset_list().front()(0);
m_limbCOPOffset[contact_states_index_map[gg->get_support_leg_list().front()]].data.y = gg->get_support_foot_zmp_offset_list().front()(1);
m_limbCOPOffset[contact_states_index_map[gg->get_support_leg_list().front()]].data.z = gg->get_support_foot_zmp_offset_list().front()(2);
m_limbCOPOffset[contact_states_index_map[gg->get_swing_legs().front()]].data.x = gg->get_swing_foot_zmp_offsets().front()(0);
m_limbCOPOffset[contact_states_index_map[gg->get_swing_legs().front()]].data.y = gg->get_swing_foot_zmp_offsets().front()(1);
m_limbCOPOffset[contact_states_index_map[gg->get_swing_legs().front()]].data.z = gg->get_swing_foot_zmp_offsets().front()(2);
m_limbCOPOffset[contact_states_index_map[gg->get_support_legs().front()]].data.x = gg->get_support_foot_zmp_offsets().front()(0);
m_limbCOPOffset[contact_states_index_map[gg->get_support_legs().front()]].data.y = gg->get_support_foot_zmp_offsets().front()(1);
m_limbCOPOffset[contact_states_index_map[gg->get_support_legs().front()]].data.z = gg->get_support_foot_zmp_offsets().front()(2);
} else {
tmp_fix_coords = fix_leg_coords;
// double support by default
Expand Down Expand Up @@ -795,7 +795,7 @@ void AutoBalancer::solveLimbIK ()
for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
if (it->second.is_active && (it->first.find("leg") != std::string::npos) && it->second.manip->numJoints() == 7) {
int i = it->second.target_link->jointId;
if (gg->get_swing_leg_list().front() == it->first) {
if (gg->get_swing_legs().front() == it->first) {
m_robot->joint(i)->q = qrefv[i] + -1 * gg->get_foot_dif_rot_angle();
} else {
m_robot->joint(i)->q = qrefv[i];
Expand Down Expand Up @@ -892,8 +892,8 @@ void AutoBalancer::startWalking ()
}
{
Guard guard(m_mutex);
std::string init_support_leg (gg->get_footstep_front_leg_list().front() == "rleg" ? "lleg" : "rleg");
std::string init_swing_leg (gg->get_footstep_front_leg_list().front());
std::string init_support_leg (gg->get_footstep_front_legs().front() == "rleg" ? "lleg" : "rleg");
std::string init_swing_leg (gg->get_footstep_front_legs().front());
gg->set_default_zmp_offsets(default_zmp_offsets);
gg->initialize_gait_parameter(ref_cog, boost::assign::list_of(ikp[init_support_leg].target_end_coords), boost::assign::list_of(ikp[init_swing_leg].target_end_coords));
}
Expand All @@ -908,7 +908,7 @@ void AutoBalancer::stopWalking ()
{
mid_coords(fix_leg_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords);
fixLegToCoords(fix_leg_coords.pos, fix_leg_coords.rot);
gg->clear_footstep_node_list_list();
gg->clear_footstep_nodes_list();
if (return_control_mode == MODE_IDLE) stopABCparam();
gg_is_walking = false;
}
Expand Down Expand Up @@ -944,7 +944,7 @@ void AutoBalancer::waitABCTransition()
bool AutoBalancer::goPos(const double& x, const double& y, const double& th)
{
if ( !gg_is_walking && !is_stop_mode) {
gg->go_pos_param_2_footstep_list_list(x, y, th,
gg->go_pos_param_2_footstep_nodes_list(x, y, th,
(y > 0 ? ikp["rleg"].target_end_coords : ikp["lleg"].target_end_coords),
(y > 0 ? ikp["lleg"].target_end_coords : ikp["rleg"].target_end_coords),
(y > 0 ? RLEG : LLEG));
Expand Down Expand Up @@ -1060,7 +1060,7 @@ bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::Foo
}
if (gg_is_walking) {
std::cerr << "[" << m_profile.instance_name << "] Set overwrite footsteps" << std::endl;
gg->set_overwrite_foot_steps(fnll);
gg->set_overwrite_foot_steps_list(fnll);
gg->set_overwrite_foot_step_index(overwrite_fs_idx);
} else {
std::cerr << "[" << m_profile.instance_name << "] Set normal footsteps" << std::endl;
Expand Down Expand Up @@ -1283,17 +1283,17 @@ bool AutoBalancer::getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam&
}
copyRatscoords2Footstep(i_param.rleg_coords, leg_coords[0]);
copyRatscoords2Footstep(i_param.lleg_coords, leg_coords[1]);
copyRatscoords2Footstep(i_param.support_leg_coords, gg->get_support_leg_coords_list().front());
copyRatscoords2Footstep(i_param.swing_leg_coords, gg->get_swing_leg_coords_list().front());
copyRatscoords2Footstep(i_param.swing_leg_src_coords, gg->get_swing_leg_src_coords_list().front());
copyRatscoords2Footstep(i_param.swing_leg_dst_coords, gg->get_swing_leg_dst_coords_list().front());
copyRatscoords2Footstep(i_param.dst_foot_midcoords, gg->get_dst_foot_midcoords_list().front());
if (gg->get_support_leg_list().front() == "rleg") {
copyRatscoords2Footstep(i_param.support_leg_coords, gg->get_support_legs_coords().front());
copyRatscoords2Footstep(i_param.swing_leg_coords, gg->get_swing_legs_coords().front());
copyRatscoords2Footstep(i_param.swing_leg_src_coords, gg->get_swing_legs_src_coords().front());
copyRatscoords2Footstep(i_param.swing_leg_dst_coords, gg->get_swing_legs_dst_coords().front());
copyRatscoords2Footstep(i_param.dst_foot_midcoords, gg->get_dst_feet_midcoords().front());
if (gg->get_support_legs().front() == "rleg") {
i_param.support_leg = OpenHRP::AutoBalancerService::RLEG;
} else {
i_param.support_leg = OpenHRP::AutoBalancerService::LLEG;
}
switch ( gg->get_current_support_state_list().front() ) {
switch ( gg->get_current_support_states().front() ) {
case BOTH: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::BOTH; break;
case RLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::RLEG; break;
case LLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::LLEG; break;
Expand Down Expand Up @@ -1370,7 +1370,7 @@ bool AutoBalancer::getRemainingFootstepSequence(OpenHRP::AutoBalancerService::Fo
std::cerr << "[" << m_profile.instance_name << "] getRemainingFootstepSequence" << std::endl;
o_footstep = new OpenHRP::AutoBalancerService::FootstepSequence;
if (gg_is_walking) {
std::vector< std::vector<step_node> > fsll = gg->get_remaining_footstep_list_list();
std::vector< std::vector<step_node> > fsll = gg->get_remaining_footstep_nodes_list();
o_current_fs_idx = gg->get_footstep_index();
o_footstep->length(fsll.size());
for (size_t i = 0; i < fsll.size(); i++) {
Expand Down Expand Up @@ -1436,9 +1436,9 @@ hrp::Vector3 AutoBalancer::calc_vel_from_hand_error (const coordinates& tmp_fix_
{
if (graspless_manip_mode) {
hrp::Vector3 dp,dr;
coordinates ref_hand_coords(gg->get_dst_foot_midcoords_list().front()), act_hand_coords;
coordinates ref_hand_coords(gg->get_dst_feet_midcoords().front()), act_hand_coords;
ref_hand_coords.transform(graspless_manip_reference_trans_coords); // desired arm coords
hrp::Vector3 foot_pos(gg->get_dst_foot_midcoords_list().front().pos);
hrp::Vector3 foot_pos(gg->get_dst_feet_midcoords().front().pos);
if ( graspless_manip_arm == "arms" ) {
// act_hand_coords.pos = (target_coords["rarm"].pos + target_coords["larm"].pos) / 2.0;
// vector3 cur_y(target_coords["larm"].pos - target_coords["rarm"].pos);
Expand All @@ -1458,7 +1458,7 @@ hrp::Vector3 AutoBalancer::calc_vel_from_hand_error (const coordinates& tmp_fix_
dp = act_hand_coords.pos - ref_hand_coords.pos
+ dr.cross(hrp::Vector3(foot_pos - act_hand_coords.pos));
dp(2) = 0;
hrp::Matrix33 foot_mt(gg->get_dst_foot_midcoords_list().front().rot.transpose());
hrp::Matrix33 foot_mt(gg->get_dst_feet_midcoords().front().rot.transpose());
//alias(dp) = foot_mt * dp;
hrp::Vector3 dp2 = foot_mt * dp;
//alias(dr) = foot_mt * dr;
Expand Down
Loading

0 comments on commit 435c1aa

Please sign in to comment.