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 gopos #877

Merged
merged 9 commits into from
Dec 20, 2015
Merged

Update gopos #877

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
c7f7f5b
[rtc/*] Update print message from RTCs like [el]
snozawa Dec 19, 2015
216e554
[sample/SampleRobot/samplerobot_impedance_controller.py, test/test-sa…
snozawa Dec 19, 2015
7c8440b
[rtc/ImpedanceController/JointPathEx.*, AutoBalancer, ImpedanceContro…
snozawa Dec 20, 2015
c3429cb
[sample/SampleRobot/samplerobot_impedance_controller.py] Check hrpsys…
snozawa Dec 20, 2015
5bacb5a
[rtc/AutoBalancer/GaitGenerator.*] Update appending of footstep funct…
snozawa Dec 20, 2015
c758861
[rtc/AutoBalancer/GaitGenerator.*, AutoBalancer.cpp] Enable to overwr…
snozawa Dec 20, 2015
69d69a6
[sample/SampleRobot/samplerobot_auto_balancer.py] Add check test for …
snozawa Dec 20, 2015
0f372c6
[rtc/AutoBalancer/GaitGenerator.*] Use const member function for gett…
snozawa Dec 20, 2015
faa615d
[rtc/AutoBalancer/GaitGenerator.h] Set is_initialize for gopos true b…
snozawa Dec 20, 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
Prev Previous commit
Next Next commit
[rtc/AutoBalancer/GaitGenerator.*] Update appending of footstep funct… 
…ion. Define both const and non-const member function.
  • Loading branch information
@snozawa
snozawa committed Dec 20, 2015
commit 5bacb5aaae326a5b6a6070ef3215f15cee794ae5
12 changes: 9 additions & 3 deletions rtc/AutoBalancer/GaitGenerator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -508,6 +508,7 @@ namespace rats
}
}
}
// rg+lcg initialization
rg.reset(one_step_len);
rg.push_refzmp_from_footstep_nodes_for_dual(footstep_nodes_list.front(), initial_support_leg_steps, initial_swing_leg_dst_steps);
if ( preview_controller_ptr != NULL ) {
Expand Down Expand Up @@ -695,7 +696,7 @@ namespace rats
if (velocity_mode_flg == VEL_DOING) velocity_mode_flg = VEL_ENDING;
};

void gait_generator::calc_ref_coords_trans_vector_velocity_mode (coordinates& ref_coords, hrp::Vector3& trans, double& dth, const std::vector<step_node>& sup_fns)
void gait_generator::calc_ref_coords_trans_vector_velocity_mode (coordinates& ref_coords, hrp::Vector3& trans, double& dth, const std::vector<step_node>& sup_fns) const
{
ref_coords = sup_fns.front().worldcoords;
hrp::Vector3 tmpv(footstep_param.leg_default_translate_pos[sup_fns.front().l_r] * -1.0); /* not fair to every support legs */
Expand Down Expand Up @@ -724,15 +725,20 @@ namespace rats
};

void gait_generator::append_footstep_list_velocity_mode ()
{
append_footstep_list_velocity_mode(footstep_nodes_list);
};

void gait_generator::append_footstep_list_velocity_mode (std::vector< std::vector<step_node> >& _footstep_nodes_list) const
{
coordinates ref_coords;
hrp::Vector3 trans;
double dth;
calc_ref_coords_trans_vector_velocity_mode(ref_coords, trans, dth, footstep_nodes_list.back());
calc_ref_coords_trans_vector_velocity_mode(ref_coords, trans, dth, _footstep_nodes_list.back());

ref_coords.pos += ref_coords.rot * trans;
ref_coords.rotate(dth, hrp::Vector3(0,0,1));
append_go_pos_step_nodes(ref_coords, calc_counter_leg_types_from_footstep_nodes(footstep_nodes_list.back(), all_limbs));
append_go_pos_step_nodes(ref_coords, calc_counter_leg_types_from_footstep_nodes(_footstep_nodes_list.back(), all_limbs), _footstep_nodes_list);
};

void gait_generator::calc_next_coords_velocity_mode (std::vector< std::vector<coordinates> >& ret_list, const size_t idx, const size_t future_step_num)
Expand Down
20 changes: 14 additions & 6 deletions rtc/AutoBalancer/GaitGenerator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -830,8 +830,8 @@ namespace rats
hrp::Vector3 get_cycloid_delay_kick_point_offset () { return cdktg.get_cycloid_delay_kick_point_offset() ; };
double get_toe_pos_offset_x () { return toe_pos_offset_x; };
double get_heel_pos_offset_x () { return heel_pos_offset_x; };
double get_toe_angle () { return toe_angle; };
double get_heel_angle () { return heel_angle; };
double get_toe_angle () const { return toe_angle; };
double get_heel_angle () const { return heel_angle; };
double get_foot_dif_rot_angle () { return foot_dif_rot_angle; };
bool get_use_toe_joint () { return use_toe_joint; };
};
Expand Down Expand Up @@ -885,6 +885,13 @@ namespace rats

void append_go_pos_step_nodes (const coordinates& _ref_coords,
const std::vector<leg_type>& lts)
{
append_go_pos_step_nodes(_ref_coords, lts, footstep_nodes_list);
};

void append_go_pos_step_nodes (const coordinates& _ref_coords,
const std::vector<leg_type>& lts,
std::vector< std::vector<step_node> >& _footstep_nodes_list) const
{
std::vector<step_node> sns;
for (size_t i = 0; i < lts.size(); i++) {
Expand All @@ -893,12 +900,13 @@ namespace rats
lcg.get_toe_angle(), lcg.get_heel_angle()));
sns.at(i).worldcoords.pos += sns.at(i).worldcoords.rot * footstep_param.leg_default_translate_pos[lts.at(i)];
}
footstep_nodes_list.push_back(sns);
_footstep_nodes_list.push_back(sns);
};
void overwrite_refzmp_queue(const std::vector< std::vector<step_node> >& fnsl);
void calc_ref_coords_trans_vector_velocity_mode (coordinates& ref_coords, hrp::Vector3& trans, double& dth, const std::vector<step_node>& sup_fns);
void calc_ref_coords_trans_vector_velocity_mode (coordinates& ref_coords, hrp::Vector3& trans, double& dth, const std::vector<step_node>& sup_fns) const;
void calc_next_coords_velocity_mode (std::vector< std::vector<coordinates> >& ret_list, const size_t idx, const size_t future_step_num = 3);
void append_footstep_list_velocity_mode ();
void append_footstep_list_velocity_mode (std::vector< std::vector<step_node> >& _footstep_nodes_list) const;

#ifndef HAVE_MAIN
/* inhibit copy constructor and copy insertion not by implementing */
Expand Down Expand Up @@ -1181,8 +1189,8 @@ namespace rats
double get_gravitational_acceleration () { return gravitational_acceleration; } ;
double get_toe_pos_offset_x () { return lcg.get_toe_pos_offset_x(); };
double get_heel_pos_offset_x () { return lcg.get_heel_pos_offset_x(); };
double get_toe_angle () { return lcg.get_toe_angle(); };
double get_heel_angle () { return lcg.get_heel_angle(); };
double get_toe_angle () const { return lcg.get_toe_angle(); };
double get_heel_angle () const { return lcg.get_heel_angle(); };
double get_foot_dif_rot_angle () { return lcg.get_foot_dif_rot_angle(); };
void get_toe_heel_phase_ratio (std::vector<double>& ratio) { thp.get_toe_heel_phase_ratio(ratio); };
int get_NUM_TH_PHASES () { return thp.get_NUM_TH_PHASES(); };
Expand Down