sample RT component which has one data input port and one data output port More...

#include <Stabilizer.h>

Inheritance diagram for Stabilizer:

Classes
struct	STIKParam
Public Member Functions
void	calcContactMatrix (hrp::dmatrix &tm, const std::vector< hrp::Vector3 > &contact_p)
double	calcDampingControl (const double tau_d, const double tau, const double prev_d, const double DD, const double TT)
hrp::Vector3	calcDampingControl (const hrp::Vector3 &prev_d, const hrp::Vector3 &TT)
double	calcDampingControl (const double prev_d, const double TT)
hrp::Vector3	calcDampingControl (const hrp::Vector3 &tau_d, const hrp::Vector3 &tau, const hrp::Vector3 &prev_d, const hrp::Vector3 &DD, const hrp::Vector3 &TT)
void	calcDiffFootOriginExtMoment ()
void	calcEEForceMomentControl ()
void	calcFootOriginCoords (hrp::Vector3 &foot_origin_pos, hrp::Matrix33 &foot_origin_rot)
int	calcMaxTransitionCount ()
void	calcRUNST ()
void	calcStateForEmergencySignal ()
void	calcSwingEEModification ()
void	calcSwingSupportLimbGain ()
void	calcTorque ()
void	calcTPCC ()
bool	calcZMP (hrp::Vector3 &ret_zmp, const double zmp_z)
void	fixLegToCoords (const std::string &leg, const rats::coordinates &coords)
void	getActualParameters ()
void	getCurrentParameters ()
void	getFootmidCoords (rats::coordinates &ret)
void	getParameter (OpenHRP::StabilizerService::stParam &i_stp)
std::string	getStabilizerAlgorithmString (OpenHRP::StabilizerService::STAlgorithm _st_algorithm)
void	getTargetParameters ()
bool	isContact (const size_t idx)
void	limbStretchAvoidanceControl (const std::vector< hrp::Vector3 > &ee_p, const std::vector< hrp::Matrix33 > &ee_R)
void	moveBasePosRotForBodyRPYControl ()
virtual RTC::ReturnCode_t	onActivated (RTC::UniqueId ec_id)
virtual RTC::ReturnCode_t	onDeactivated (RTC::UniqueId ec_id)
virtual RTC::ReturnCode_t	onExecute (RTC::UniqueId ec_id)
virtual RTC::ReturnCode_t	onFinalize ()
virtual RTC::ReturnCode_t	onInitialize ()
void	setBoolSequenceParam (std::vector< bool > &st_bool_values, const OpenHRP::StabilizerService::BoolSequence &output_bool_values, const std::string &prop_name)
void	setBoolSequenceParamWithCheckContact (std::vector< bool > &st_bool_values, const OpenHRP::StabilizerService::BoolSequence &output_bool_values, const std::string &prop_name)
void	setParameter (const OpenHRP::StabilizerService::stParam &i_stp)
	Stabilizer (RTC::Manager *manager)
	Constructor.
void	startStabilizer (void)
void	stopStabilizer (void)
void	sync_2_idle ()
void	sync_2_st ()
double	vlimit (double value, double llimit_value, double ulimit_value)
hrp::Vector3	vlimit (const hrp::Vector3 &value, double llimit_value, double ulimit_value)
hrp::Vector3	vlimit (const hrp::Vector3 &value, const hrp::Vector3 &limit_value)
hrp::Vector3	vlimit (const hrp::Vector3 &value, const hrp::Vector3 &llimit_value, const hrp::Vector3 &ulimit_value)
void	waitSTTransition ()
virtual	~Stabilizer ()
	Destructor.
Protected Attributes
RTC::TimedOrientation3D	m_actBaseRpy
RTC::OutPort < RTC::TimedOrientation3D >	m_actBaseRpyOut
RTC::TimedBooleanSeq	m_actContactStates
RTC::OutPort < RTC::TimedBooleanSeq >	m_actContactStatesOut
RTC::TimedPoint3D	m_actCP
RTC::OutPort< RTC::TimedPoint3D >	m_actCPOut
RTC::TimedDoubleSeq	m_allEEComp
RTC::OutPort< RTC::TimedDoubleSeq >	m_allEECompOut
RTC::TimedDoubleSeq	m_allRefWrench
RTC::OutPort< RTC::TimedDoubleSeq >	m_allRefWrenchOut
RTC::TimedPoint3D	m_basePos
RTC::InPort< RTC::TimedPoint3D >	m_basePosIn
RTC::TimedOrientation3D	m_baseRpy
RTC::InPort < RTC::TimedOrientation3D >	m_baseRpyIn
RTC::TimedBooleanSeq	m_contactStates
RTC::InPort< RTC::TimedBooleanSeq >	m_contactStatesIn
RTC::TimedDoubleSeq	m_controlSwingSupportTime
RTC::InPort< RTC::TimedDoubleSeq >	m_controlSwingSupportTimeIn
RTC::TimedDoubleSeq	m_COPInfo
RTC::OutPort< RTC::TimedDoubleSeq >	m_COPInfoOut
RTC::TimedPoint3D	m_currentBasePos
RTC::OutPort< RTC::TimedPoint3D >	m_currentBasePosOut
RTC::TimedOrientation3D	m_currentBaseRpy
RTC::OutPort < RTC::TimedOrientation3D >	m_currentBaseRpyOut
RTC::TimedDoubleSeq	m_debugData
RTC::OutPort< RTC::TimedDoubleSeq >	m_debugDataOut
RTC::TimedPoint3D	m_diffCP
RTC::OutPort< RTC::TimedPoint3D >	m_diffCPOut
RTC::TimedPoint3D	m_diffFootOriginExtMoment
RTC::OutPort< RTC::TimedPoint3D >	m_diffFootOriginExtMomentOut
RTC::TimedLong	m_emergencySignal
RTC::OutPort< RTC::TimedLong >	m_emergencySignalOut
std::vector< RTC::TimedPoint3D >	m_limbCOPOffset
std::vector< RTC::InPort < RTC::TimedPoint3D > * >	m_limbCOPOffsetIn
RTC::TimedPoint3D	m_originActCog
RTC::OutPort< RTC::TimedPoint3D >	m_originActCogOut
RTC::TimedPoint3D	m_originActCogVel
RTC::OutPort< RTC::TimedPoint3D >	m_originActCogVelOut
RTC::TimedPoint3D	m_originActZmp
RTC::OutPort< RTC::TimedPoint3D >	m_originActZmpOut
RTC::TimedPoint3D	m_originNewZmp
RTC::OutPort< RTC::TimedPoint3D >	m_originNewZmpOut
RTC::TimedPoint3D	m_originRefCog
RTC::OutPort< RTC::TimedPoint3D >	m_originRefCogOut
RTC::TimedPoint3D	m_originRefCogVel
RTC::OutPort< RTC::TimedPoint3D >	m_originRefCogVelOut
RTC::TimedPoint3D	m_originRefZmp
RTC::OutPort< RTC::TimedPoint3D >	m_originRefZmpOut
RTC::TimedDoubleSeq	m_qCurrent
RTC::InPort< RTC::TimedDoubleSeq >	m_qCurrentIn
RTC::TimedDoubleSeq	m_qRef
RTC::InPort< RTC::TimedDoubleSeq >	m_qRefIn
RTC::OutPort< RTC::TimedDoubleSeq >	m_qRefOut
RTC::TimedDoubleSeq	m_qRefSeq
RTC::InPort< RTC::TimedDoubleSeq >	m_qRefSeqIn
std::vector< RTC::TimedDoubleSeq >	m_ref_wrenches
std::vector< RTC::InPort < RTC::TimedDoubleSeq > * >	m_ref_wrenchesIn
RTC::TimedPoint3D	m_refCP
RTC::OutPort< RTC::TimedPoint3D >	m_refCPOut
RTC::TimedOrientation3D	m_rpy
RTC::InPort < RTC::TimedOrientation3D >	m_rpyIn
RTC::TimedPoint3D	m_sbpCogOffset
RTC::InPort< RTC::TimedPoint3D >	m_sbpCogOffsetIn
StabilizerService_impl	m_service0
RTC::CorbaPort	m_StabilizerServicePort
RTC::TimedDoubleSeq	m_tau
RTC::OutPort< RTC::TimedDoubleSeq >	m_tauOut
RTC::TimedDoubleSeq	m_toeheelRatio
RTC::InPort< RTC::TimedDoubleSeq >	m_toeheelRatioIn
RTC::TimedBoolean	m_walkingStates
RTC::InPort< RTC::TimedBoolean >	m_walkingStatesIn
std::vector< RTC::TimedDoubleSeq >	m_wrenches
std::vector< RTC::InPort < RTC::TimedDoubleSeq > * >	m_wrenchesIn
RTC::TimedPoint3D	m_zmp
RTC::OutPort< RTC::TimedPoint3D >	m_zmpOut
RTC::TimedPoint3D	m_zmpRef
RTC::InPort< RTC::TimedPoint3D >	m_zmpRefIn
Private Types
enum	cmode { MODE_IDLE, MODE_AIR, MODE_ST, MODE_SYNC_TO_IDLE, MODE_SYNC_TO_AIR }
Private Attributes
hrp::Vector3	act_base_rpy
hrp::Vector3	act_cog
hrp::Vector3	act_cogvel
boost::shared_ptr < FirstOrderLowPassFilter < hrp::Vector3 > >	act_cogvel_filter
std::vector< bool >	act_contact_states
hrp::Vector3	act_cp
std::vector< hrp::Vector3 >	act_ee_p
std::vector< hrp::Matrix33 >	act_ee_R
std::vector< hrp::Vector3 >	act_force
hrp::Vector3	act_total_foot_origin_moment
hrp::Vector3	act_zmp
double	contact_decision_threshold
std::map< std::string, size_t >	contact_states_index_map
enum Stabilizer::cmode	control_mode
double	cop_check_margin
std::vector< double >	cp_check_margin
hrp::Vector3	cp_offset
hrp::Vector3	current_base_pos
hrp::Vector3	current_base_rpy
hrp::Vector3	current_root_p
hrp::Matrix33	current_root_R
double	d_pos_z_root
double	d_rpy [2]
double	d_run_b [2]
int	detection_count_to_air
hrp::Vector3	diff_cp
hrp::Vector3	diff_foot_origin_ext_moment
double	dt
double	eefm_body_attitude_control_gain [2]
double	eefm_body_attitude_control_time_const [2]
double	eefm_gravitational_acceleration
double	eefm_k1 [2]
double	eefm_k2 [2]
double	eefm_k3 [2]
double	eefm_pos_margin_time
double	eefm_pos_time_const_swing
double	eefm_pos_transition_time
std::vector< double >	eefm_swing_damping_force_thre
std::vector< double >	eefm_swing_damping_moment_thre
hrp::Vector3	eefm_swing_pos_damping_gain
hrp::Vector3	eefm_swing_rot_damping_gain
bool	eefm_use_force_difference_control
bool	eefm_use_swing_damping
double	eefm_zmp_delay_time_const [2]
OpenHRP::StabilizerService::EmergencyCheckMode	emergency_check_mode
hrp::Vector3	foot_origin_offset [2]
bool	initial_cp_too_large_error
int	is_air_counter
bool	is_emergency
bool	is_estop_while_walking
std::vector< bool >	is_feedback_control_enable
std::vector< bool >	is_ik_enable
bool	is_legged_robot
bool	is_seq_interpolating
bool	is_walking
std::vector< bool >	is_zmp_calc_enable
std::vector< hrp::JointPathExPtr >	jpe_v
double	k_brot_p [2]
double	k_brot_tc [2]
double	k_run_b [2]
double	k_tpcc_p [2]
double	k_tpcc_x [2]
double	limb_stretch_avoidance_time_const
double	limb_stretch_avoidance_vlimit [2]
int	loop
unsigned int	m_debugLevel
TwoDofController	m_f_z
int	m_is_falling_counter
coil::Mutex	m_mutex
hrp::BodyPtr	m_robot
TwoDofController	m_tau_x [2]
TwoDofController	m_tau_y [2]
double	m_torque_d [2]
double	m_torque_k [2]
std::map< std::string, hrp::VirtualForceSensorParam >	m_vfs
std::vector< int >	m_will_fall_counter
std::vector< std::vector < Eigen::Vector2d > >	margined_support_polygon_vetices
hrp::Vector3	new_refzmp
bool	on_ground
double	pangx
double	pangx_ref
double	pangy
double	pangy_ref
hrp::Vector3	pdr
hrp::Vector3	pos_ctrl
hrp::Vector3	prev_act_cog
hrp::Matrix33	prev_act_foot_origin_rot
std::vector< double >	prev_act_force_z
hrp::Vector3	prev_ref_cog
std::vector< bool >	prev_ref_contact_states
hrp::Matrix33	prev_ref_foot_origin_rot
hrp::Vector3	prev_ref_zmp
std::vector< hrp::Vector3 >	projected_normal
hrp::dvector	qorg
hrp::dvector	qrefv
double	rdx
double	rdy
hrp::Vector3	ref_cog
hrp::Vector3	ref_cogvel
std::vector< bool >	ref_contact_states
hrp::Vector3	ref_cp
hrp::Matrix33	ref_foot_origin_rot
std::vector< hrp::Vector3 >	ref_force
std::vector< hrp::Vector3 >	ref_moment
hrp::Vector3	ref_total_foot_origin_moment
hrp::Vector3	ref_total_force
hrp::Vector3	ref_total_moment
hrp::Vector3	ref_zmp
hrp::Vector3	ref_zmp_aux
hrp::Vector3	rel_act_cp
hrp::Vector3	rel_act_zmp
hrp::Vector3	rel_cog
std::vector< std::string >	rel_ee_name
std::vector< hrp::Vector3 >	rel_ee_pos
std::vector< hrp::Matrix33 >	rel_ee_rot
hrp::Vector3	rel_ref_cp
bool	reset_emergency_flag
double	root_rot_compensation_limit [2]
double	rx
double	ry
hrp::Vector3	sbp_cog_offset
OpenHRP::StabilizerService::STAlgorithm	st_algorithm
std::vector< STIKParam >	stikp
std::vector< std::vector < Eigen::Vector2d > >	support_polygon_vetices
SimpleZMPDistributor *	szd
std::vector< hrp::Vector3 >	target_ee_p
std::vector< hrp::Matrix33 >	target_ee_R
rats::coordinates	target_foot_midcoords
hrp::Matrix33	target_foot_origin_rot
hrp::Vector3	target_root_p
hrp::Matrix33	target_root_R
std::vector< double >	tilt_margin
std::vector< double >	toeheel_ratio
double	total_mass
int	transition_count
hrp::dvector	transition_joint_q
double	transition_smooth_gain
double	transition_time
bool	use_limb_stretch_avoidance
bool	use_zmp_truncation
double	zmp_origin_off

Detailed Description

sample RT component which has one data input port and one data output port

Definition at line 44 of file Stabilizer.h.

Member Enumeration Documentation

enum Stabilizer::cmode [private]

Enumerator:

MODE_IDLE
MODE_AIR
MODE_ST
MODE_SYNC_TO_IDLE
MODE_SYNC_TO_AIR

Definition at line 281 of file Stabilizer.h.

Constructor & Destructor Documentation

Stabilizer::Stabilizer ( RTC::Manager * manager )

Constructor.

Parameters:

manager pointer to the Manager

Definition at line 61 of file Stabilizer.cpp.

Stabilizer::~Stabilizer ( ) [virtual]

Destructor.

Definition at line 111 of file Stabilizer.cpp.

Member Function Documentation

void Stabilizer::calcContactMatrix	(	hrp::dmatrix &	tm,
		const std::vector< hrp::Vector3 > &	contact_p
	)

Definition at line 2722 of file Stabilizer.cpp.

double Stabilizer::calcDampingControl	(	const double	tau_d,
		const double	tau,
		const double	prev_d,
		const double	DD,
		const double	TT
	)

Definition at line 1722 of file Stabilizer.cpp.

hrp::Vector3 Stabilizer::calcDampingControl	(	const hrp::Vector3 &	prev_d,
		const hrp::Vector3 &	TT
	)

Definition at line 1729 of file Stabilizer.cpp.

double Stabilizer::calcDampingControl	(	const double	prev_d,
		const double	TT
	)

Definition at line 1735 of file Stabilizer.cpp.

hrp::Vector3 Stabilizer::calcDampingControl	(	const hrp::Vector3 &	tau_d,
		const hrp::Vector3 &	tau,
		const hrp::Vector3 &	prev_d,
		const hrp::Vector3 &	DD,
		const hrp::Vector3 &	TT
	)

Definition at line 1740 of file Stabilizer.cpp.

void Stabilizer::calcDiffFootOriginExtMoment ( )

Definition at line 1746 of file Stabilizer.cpp.

void Stabilizer::calcEEForceMomentControl ( )

Definition at line 1539 of file Stabilizer.cpp.

void Stabilizer::calcFootOriginCoords	(	hrp::Vector3 &	foot_origin_pos,
		hrp::Matrix33 &	foot_origin_rot
	)

Definition at line 768 of file Stabilizer.cpp.

int Stabilizer::calcMaxTransitionCount ( ) [inline]

Definition at line 149 of file Stabilizer.h.

void Stabilizer::calcRUNST ( )

Definition at line 2537 of file Stabilizer.cpp.

void Stabilizer::calcStateForEmergencySignal ( )

Definition at line 1308 of file Stabilizer.cpp.

void Stabilizer::calcSwingEEModification ( )

Definition at line 1652 of file Stabilizer.cpp.

void Stabilizer::calcSwingSupportLimbGain ( )

Definition at line 1459 of file Stabilizer.cpp.

void Stabilizer::calcTorque ( )

Definition at line 2735 of file Stabilizer.cpp.

void Stabilizer::calcTPCC ( )

Definition at line 1495 of file Stabilizer.cpp.

bool Stabilizer::calcZMP	(	hrp::Vector3 &	ret_zmp,
		const double	zmp_z
	)

Definition at line 1267 of file Stabilizer.cpp.

void Stabilizer::fixLegToCoords	(	const std::string &	leg,
		const rats::coordinates &	coords
	)

void Stabilizer::getActualParameters ( )

Definition at line 799 of file Stabilizer.cpp.

void Stabilizer::getCurrentParameters ( )

Definition at line 759 of file Stabilizer.cpp.

void Stabilizer::getFootmidCoords ( rats::coordinates & ret )

void Stabilizer::getParameter ( OpenHRP::StabilizerService::stParam & i_stp )

Definition at line 1872 of file Stabilizer.cpp.

std::string Stabilizer::getStabilizerAlgorithmString ( OpenHRP::StabilizerService::STAlgorithm _st_algorithm )

Definition at line 2362 of file Stabilizer.cpp.

void Stabilizer::getTargetParameters ( )

Definition at line 1152 of file Stabilizer.cpp.

bool Stabilizer::isContact ( const size_t idx ) [inline]

Definition at line 145 of file Stabilizer.h.

void Stabilizer::limbStretchAvoidanceControl	(	const std::vector< hrp::Vector3 > &	ee_p,
		const std::vector< hrp::Matrix33 > &	ee_R
	)

Definition at line 1695 of file Stabilizer.cpp.

void Stabilizer::moveBasePosRotForBodyRPYControl ( )

Definition at line 1432 of file Stabilizer.cpp.

RTC::ReturnCode_t Stabilizer::onActivated ( RTC::UniqueId ec_id ) [virtual]

Reimplemented from RTC::RTObject_impl.

Definition at line 528 of file Stabilizer.cpp.

RTC::ReturnCode_t Stabilizer::onDeactivated ( RTC::UniqueId ec_id ) [virtual]

Reimplemented from RTC::RTObject_impl.

Definition at line 534 of file Stabilizer.cpp.

RTC::ReturnCode_t Stabilizer::onExecute ( RTC::UniqueId ec_id ) [virtual]

Reimplemented from RTC::RTObject_impl.

Definition at line 548 of file Stabilizer.cpp.

RTC::ReturnCode_t Stabilizer::onFinalize ( void ) [virtual]

Reimplemented from RTC::RTObject_impl.

Definition at line 505 of file Stabilizer.cpp.

RTC::ReturnCode_t Stabilizer::onInitialize ( void ) [virtual]

Reimplemented from RTC::RTObject_impl.

Definition at line 115 of file Stabilizer.cpp.

void Stabilizer::setBoolSequenceParam	(	std::vector< bool > &	st_bool_values,
		const OpenHRP::StabilizerService::BoolSequence &	output_bool_values,
		const std::string &	prop_name
	)

Definition at line 2380 of file Stabilizer.cpp.

void Stabilizer::setBoolSequenceParamWithCheckContact	(	std::vector< bool > &	st_bool_values,
		const OpenHRP::StabilizerService::BoolSequence &	output_bool_values,
		const std::string &	prop_name
	)