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

Update st abc kf param #549

Merged
merged 2 commits into from
Mar 31, 2015
Merged

Update st abc kf param #549

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
aaf6c15
Update default parameters for ST, KF, ABC
snozawa Mar 31, 2015
1d84c7c
Update print message for parameters
snozawa Mar 31, 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
8 changes: 4 additions & 4 deletions rtc/AutoBalancer/AutoBalancer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,7 @@ AutoBalancer::~AutoBalancer()

RTC::ReturnCode_t AutoBalancer::onInitialize()
{
std::cout << "AutoBalancer::onInitialize()" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
bindParameter("debugLevel", m_debugLevel, "0");

// Registration: InPort/OutPort/Service
Expand Down Expand Up @@ -153,7 +153,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
if (leg_offset_str.size() > 0) {
hrp::Vector3 leg_offset;
for (size_t i = 0; i < 3; i++) coil::stringTo(leg_offset(i), leg_offset_str[i].c_str());
std::cerr << m_profile.instance_name << " abc_leg_offset = " << leg_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] abc_leg_offset = " << leg_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
leg_pos.push_back(hrp::Vector3(-1*leg_offset));
leg_pos.push_back(hrp::Vector3(leg_offset));
}
Expand Down Expand Up @@ -308,14 +308,14 @@ RTC::ReturnCode_t AutoBalancer::onFinalize()

RTC::ReturnCode_t AutoBalancer::onActivated(RTC::UniqueId ec_id)
{
std::cout << "AutoBalancer::onActivated(" << ec_id << ")" << std::endl;
std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;

return RTC::RTC_OK;
}

RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
{
std::cout << "AutoBalancer::onDeactivated(" << ec_id << ")" << std::endl;
std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
Guard guard(m_mutex);
if (control_mode == MODE_ABC) {
control_mode = MODE_SYNC_TO_IDLE;
Expand Down
2 changes: 1 addition & 1 deletion rtc/AutoBalancer/GaitGenerator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -132,7 +132,7 @@ namespace rats
double total_time, time_offset, current_time, double_support_time_half; // [s]
virtual hrp::Vector3 interpolate_antecedent_path (const hrp::Vector3& start, const hrp::Vector3& goal, const double height, const double tmp_ratio) = 0;
public:
delay_hoffarbib_trajectory_generator () : total_time(0), time_offset(0.2), current_time(0) {};
delay_hoffarbib_trajectory_generator () : total_time(0), time_offset(0.35), current_time(0) {};
~delay_hoffarbib_trajectory_generator() { };
void set_dt (const double __dt) { _dt = __dt; };
void set_swing_trajectory_delay_time_offset (const double _time_offset) { time_offset = _time_offset; };
Expand Down
8 changes: 4 additions & 4 deletions rtc/KalmanFilter/KalmanFilter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ KalmanFilter::~KalmanFilter()
#define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
RTC::ReturnCode_t KalmanFilter::onInitialize()
{
std::cerr << m_profile.instance_name << ": onInitialize()" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
// <rtc-template block="bind_config">
// Bind variables and configuration variable
bindParameter("debugLevel", m_debugLevel, "0");
Expand Down Expand Up @@ -126,7 +126,7 @@ RTC::ReturnCode_t KalmanFilter::onInitialize()
} else {
m_sensorR = hrp::Matrix33::Identity();
}
rpy_kf.setParam(m_dt, 0.001, 0.003, 0.03);
rpy_kf.setParam(m_dt, 0.001, 0.003, 1000, std::string(m_profile.instance_name));
rpy_kf.setSensorR(m_sensorR);
ekf_filter.setdt(m_dt);
kf_algorithm = OpenHRP::KalmanFilterService::RPYKalmanFilter;
Expand Down Expand Up @@ -159,13 +159,13 @@ RTC::ReturnCode_t KalmanFilter::onInitialize()

RTC::ReturnCode_t KalmanFilter::onActivated(RTC::UniqueId ec_id)
{
std::cerr << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
return RTC::RTC_OK;
}

RTC::ReturnCode_t KalmanFilter::onDeactivated(RTC::UniqueId ec_id)
{
std::cerr << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
return RTC::RTC_OK;
}

Expand Down
6 changes: 3 additions & 3 deletions rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ RemoveForceSensorLinkOffset::~RemoveForceSensorLinkOffset()

RTC::ReturnCode_t RemoveForceSensorLinkOffset::onInitialize()
{
std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
// <rtc-template block="bind_config">
// Bind variables and configuration variable
bindParameter("debugLevel", m_debugLevel, "0");
Expand Down Expand Up @@ -137,13 +137,13 @@ RTC::ReturnCode_t RemoveForceSensorLinkOffset::onShutdown(RTC::UniqueId ec_id)

RTC::ReturnCode_t RemoveForceSensorLinkOffset::onActivated(RTC::UniqueId ec_id)
{
std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
return RTC::RTC_OK;
}

RTC::ReturnCode_t RemoveForceSensorLinkOffset::onDeactivated(RTC::UniqueId ec_id)
{
std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
return RTC::RTC_OK;
}

Expand Down
25 changes: 14 additions & 11 deletions rtc/Stabilizer/Stabilizer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@ Stabilizer::~Stabilizer()

RTC::ReturnCode_t Stabilizer::onInitialize()
{
std::cerr << m_profile.instance_name << ": onInitialize()" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
// <rtc-template block="bind_config">
// Bind variables and configuration variable
bindParameter("debugLevel", m_debugLevel, "0");
Expand Down Expand Up @@ -166,12 +166,12 @@ RTC::ReturnCode_t Stabilizer::onInitialize()
m_wrenchesIn.resize(npforce);
m_limbCOPOffset.resize(npforce);
m_limbCOPOffsetIn.resize(npforce);
std::cerr << "[" << m_profile.instance_name << "] force sensor ports (" << npforce << ")" << std::endl;
for (unsigned int i=0; i<npforce; ++i){
hrp::Sensor *s = m_robot->sensor(hrp::Sensor::FORCE, i);
m_wrenchesIn[i] = new RTC::InPort<RTC::TimedDoubleSeq>(std::string(s->name+"Ref").c_str(), m_wrenches[i]);
m_wrenches[i].data.length(6);
registerInPort(std::string(s->name+"Ref").c_str(), *m_wrenchesIn[i]);
std::cerr << "[" << m_profile.instance_name << "] force sensor" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] name = " << s->name << std::endl;
}
std::cerr << "[" << m_profile.instance_name << "] limbCOPOffset ports (" << npforce << ")" << std::endl;
Expand Down Expand Up @@ -224,6 +224,9 @@ RTC::ReturnCode_t Stabilizer::onInitialize()
target_ee_R.push_back(hrp::Matrix33::Identity());
contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
is_ik_enable.push_back( (ee_name.find("leg") != std::string::npos ? true : false) ); // Hands ik => disabled, feet ik => enabled, by default
std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] target = " << m_robot->link(ikp.target_name)->name << ", base = " << ee_base << std::endl;
std::cerr << "[" << m_profile.instance_name << "] offset_pos = " << ikp.localp.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
}
m_contactStates.data.length(num);
}
Expand All @@ -242,21 +245,21 @@ RTC::ReturnCode_t Stabilizer::onInitialize()
eefm_k1[i] = -1.41429*k_ratio;
eefm_k2[i] = -0.404082*k_ratio;
eefm_k3[i] = -0.18*k_ratio;
eefm_body_attitude_control_gain[i] = 1.0;
eefm_body_attitude_control_gain[i] = 0.5;
eefm_body_attitude_control_time_const[i] = 1e5;
}
eefm_rot_damping_gain = 20*5;
eefm_rot_time_const = 1;
eefm_rot_time_const = 1.5;
eefm_pos_damping_gain = hrp::Vector3(3500*10, 3500*10, 3500);
eefm_pos_time_const_support = 1;
eefm_pos_time_const_swing = 0.04;
eefm_pos_transition_time = 0.02;
eefm_pos_time_const_support = 1.5;
eefm_pos_time_const_swing = 0.08;
eefm_pos_transition_time = 0.01;
eefm_pos_margin_time = 0.02;
eefm_zmp_delay_time_const[0] = eefm_zmp_delay_time_const[1] = 0.04;
eefm_zmp_delay_time_const[0] = eefm_zmp_delay_time_const[1] = 0.055;
eefm_leg_inside_margin = 0.065; // [m]
eefm_leg_front_margin = 0.05;
eefm_leg_rear_margin = 0.05;
eefm_cogvel_cutoff_freq = 35.3678; //[Hz]
eefm_cogvel_cutoff_freq = 4.0; //[Hz]
eefm_wrench_alpha_blending = 1.0; // fz_alpha

// parameters for RUNST
Expand Down Expand Up @@ -339,13 +342,13 @@ RTC::ReturnCode_t Stabilizer::onShutdown(RTC::UniqueId ec_id)

RTC::ReturnCode_t Stabilizer::onActivated(RTC::UniqueId ec_id)
{
std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
return RTC::RTC_OK;
}

RTC::ReturnCode_t Stabilizer::onDeactivated(RTC::UniqueId ec_id)
{
std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
if ( (control_mode == MODE_ST || control_mode == MODE_AIR) ) {
sync_2_idle ();
control_mode = MODE_IDLE;
Expand Down