#include <AutoBalancer.h>

Inheritance diagram for AutoBalancer:

Classes
struct	ABCIKparam
Public Member Functions
bool	adjustFootSteps (const OpenHRP::AutoBalancerService::Footstep &rfootstep, const OpenHRP::AutoBalancerService::Footstep &lfootstep)
	AutoBalancer (RTC::Manager *manager)
void	distributeReferenceZMPToWrenches (const hrp::Vector3 &_ref_zmp)
bool	emergencyStop ()
bool	getAutoBalancerParam (OpenHRP::AutoBalancerService::AutoBalancerParam &i_param)
bool	getFootstepParam (OpenHRP::AutoBalancerService::FootstepParam &i_param)
bool	getGaitGeneratorParam (OpenHRP::AutoBalancerService::GaitGeneratorParam &i_param)
bool	getGoPosFootstepsSequence (const double &x, const double &y, const double &th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep)
bool	getRemainingFootstepSequence (OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long &o_current_fs_idx)
bool	goPos (const double &x, const double &y, const double &th)
bool	goStop ()
bool	goVelocity (const double &vx, const double &vy, const double &vth)
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 ()
bool	releaseEmergencyStop ()
bool	setAutoBalancerParam (const OpenHRP::AutoBalancerService::AutoBalancerParam &i_param)
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)
bool	setGaitGeneratorParam (const OpenHRP::AutoBalancerService::GaitGeneratorParam &i_param)
bool	startAutoBalancer (const ::OpenHRP::AutoBalancerService::StrSequence &limbs)
bool	stopAutoBalancer ()
void	waitFootSteps ()
void	waitFootStepsEarly (const double tm)
virtual	~AutoBalancer ()
Protected Attributes
TimedAcceleration3D	m_accRef
OutPort< TimedAcceleration3D >	m_accRefOut
TimedBooleanSeq	m_actContactStates
InPort< TimedBooleanSeq >	m_actContactStatesIn
RTC::CorbaPort	m_AutoBalancerServicePort
TimedPoint3D	m_basePos
TimedPose3D	m_basePose
OutPort< TimedPose3D >	m_basePoseOut
InPort< TimedPoint3D >	m_basePosIn
OutPort< TimedPoint3D >	m_basePosOut
TimedOrientation3D	m_baseRpy
InPort< TimedOrientation3D >	m_baseRpyIn
OutPort< TimedOrientation3D >	m_baseRpyOut
TimedDoubleSeq	m_baseTform
OutPort< TimedDoubleSeq >	m_baseTformOut
TimedPoint3D	m_cog
OutPort< TimedPoint3D >	m_cogOut
TimedBooleanSeq	m_contactStates
OutPort< TimedBooleanSeq >	m_contactStatesOut
TimedDoubleSeq	m_controlSwingSupportTime
OutPort< TimedDoubleSeq >	m_controlSwingSupportTimeOut
TimedPoint3D	m_diffCP
InPort< TimedPoint3D >	m_diffCPIn
TimedLong	m_emergencySignal
InPort< TimedLong >	m_emergencySignalIn
std::vector< TimedDoubleSeq >	m_force
std::vector< TimedPoint3D >	m_limbCOPOffset
std::vector< OutPort < TimedPoint3D > * >	m_limbCOPOffsetOut
TimedDoubleSeq	m_optionalData
InPort< TimedDoubleSeq >	m_optionalDataIn
OutPort< TimedDoubleSeq >	m_qOut
TimedDoubleSeq	m_qRef
InPort< TimedDoubleSeq >	m_qRefIn
std::vector< TimedDoubleSeq >	m_ref_force
std::vector< InPort < TimedDoubleSeq > * >	m_ref_forceIn
std::vector< OutPort < TimedDoubleSeq > * >	m_ref_forceOut
TimedPoint3D	m_refFootOriginExtMoment
InPort< TimedPoint3D >	m_refFootOriginExtMomentIn
TimedBoolean	m_refFootOriginExtMomentIsHoldValue
InPort< TimedBoolean >	m_refFootOriginExtMomentIsHoldValueIn
TimedPoint3D	m_sbpCogOffset
OutPort< TimedPoint3D >	m_sbpCogOffsetOut
AutoBalancerService_impl	m_service0
TimedDoubleSeq	m_toeheelRatio
OutPort< TimedDoubleSeq >	m_toeheelRatioOut
TimedBoolean	m_walkingStates
OutPort< TimedBoolean >	m_walkingStatesOut
TimedPoint3D	m_zmp
InPort< TimedPoint3D >	m_zmpIn
RTC::OutPort< RTC::TimedPoint3D >	m_zmpOut
Private Types
enum	{ BIPED, TROT, PACE, CRAWL, GALLOP }
enum	{ MODE_IDLE, MODE_ABC, MODE_SYNC_TO_IDLE, MODE_SYNC_TO_ABC }
enum	{ MODE_NO_FORCE, MODE_REF_FORCE, MODE_REF_FORCE_WITH_FOOT, MODE_REF_FORCE_RFU_EXT_MOMENT }
typedef boost::shared_ptr < SimpleFullbodyInverseKinematicsSolver >	fikPtr
typedef boost::shared_ptr < rats::gait_generator >	ggPtr
Private Member Functions
bool	calc_inital_support_legs (const double &y, std::vector< rats::coordinates > &initial_support_legs_coords, std::vector< rats::leg_type > &initial_support_legs, rats::coordinates &start_ref_coords)
void	calc_static_balance_point_from_forces (hrp::Vector3 &sb_point, const hrp::Vector3 &tmpcog, const double ref_com_height, std::vector< hrp::Vector3 > &tmp_forces)
hrp::Vector3	calc_vel_from_hand_error (const rats::coordinates &tmp_fix_coords)
void	calcFixCoordsForAdjustFootstep (rats::coordinates &tmp_fix_coords)
hrp::Vector3	calcFootMidPosUsingZMPWeightMap ()
void	calcReferenceJointAnglesForIK ()
void	calculateOutputRefForces ()
void	copyRatscoords2Footstep (OpenHRP::AutoBalancerService::Footstep &out_fs, const rats::coordinates &in_fs)
void	fixLegToCoords (const hrp::Vector3 &fix_pos, const hrp::Matrix33 &fix_rot)
void	fixLegToCoords2 (rats::coordinates &tmp_fix_coords)
void	getOutputParametersForABC ()
void	getOutputParametersForIDLE ()
void	getOutputParametersForWalking ()
void	getTargetParameters ()
std::string	getUseForceModeString ()
void	interpolateLegNamesAndZMPOffsets ()
bool	isOptionalDataContact (const std::string &ee_name)
hrp::Matrix33	OrientRotationMatrix (const hrp::Matrix33 &rot, const hrp::Vector3 &axis1, const hrp::Vector3 &axis2)
void	rotateRefForcesForFixCoords (rats::coordinates &tmp_fix_coords)
void	solveFullbodyIK ()
void	startABCparam (const ::OpenHRP::AutoBalancerService::StrSequence &limbs)
bool	startWalking ()
void	static_balance_point_proc_one (hrp::Vector3 &tmp_input_sbp, const double ref_com_height)
void	stopABCparam ()
void	stopWalking ()
void	updateTargetCoordsForHandFixMode (rats::coordinates &tmp_fix_coords)
void	updateWalkingVelocityFromHandError (rats::coordinates &tmp_fix_coords)
void	waitABCTransition ()
Private Attributes
hrp::Link *	additional_force_applied_link
hrp::Vector3	additional_force_applied_point_offset
interpolator *	adjust_footstep_interpolator
double	adjust_footstep_transition_time
std::map< std::string, size_t >	contact_states_index_map
enum AutoBalancer:: { ... }	control_mode
double	d_pos_z_root
std::vector< hrp::Vector3 >	default_zmp_offsets
std::vector< std::string >	ee_vec
fikPtr	fik
rats::coordinates	fix_leg_coords
rats::coordinates	fix_leg_coords2
enum AutoBalancer:: { ... }	gait_type
ggPtr	gg
bool	gg_is_walking
bool	gg_solved
std::string	graspless_manip_arm
bool	graspless_manip_mode
hrp::Vector3	graspless_manip_p_gain
rats::coordinates	graspless_manip_reference_trans_coords
hrp::Vector3	hand_fix_initial_offset
hrp::InvDynStateBuffer	idsb
std::map< std::string, ABCIKparam >	ikp
hrp::Vector3	input_basePos
hrp::Matrix33	input_baseRot
hrp::Vector3	input_zmp
std::vector< IIRFilter >	invdyn_zmp_filters
bool	is_hand_fix_initial
bool	is_hand_fix_mode
bool	is_legged_robot
bool	is_stop_mode
std::vector< std::string >	leg_names
interpolator *	leg_names_interpolator
double	leg_names_interpolator_ratio
double	limb_stretch_avoidance_time_const
double	limb_stretch_avoidance_vlimit [2]
int	loop
unsigned int	m_debugLevel
double	m_dt
coil::Mutex	m_mutex
hrp::BodyPtr	m_robot
std::map< std::string, hrp::VirtualForceSensorParam >	m_vfs
hrp::Vector3	prev_imu_sensor_pos
hrp::Vector3	prev_imu_sensor_vel
hrp::Vector3	prev_ref_zmp
hrp::Vector3	ref_cog
std::vector< hrp::Vector3 >	ref_forces
hrp::Vector3	ref_zmp
hrp::Vector3	sbp_cog_offset
hrp::Vector3	sbp_offset
std::vector< std::string >	sensor_names
hrp::Vector3	target_root_p
hrp::Matrix33	target_root_R
interpolator *	transition_interpolator
double	transition_interpolator_ratio
double	transition_time
enum AutoBalancer:: { ... }	use_force
bool	use_limb_stretch_avoidance
interpolator *	zmp_offset_interpolator
double	zmp_transition_time

Detailed Description

Definition at line 42 of file AutoBalancer.h.

Member Typedef Documentation

typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> AutoBalancer::fikPtr [private]

Definition at line 249 of file AutoBalancer.h.

typedef boost::shared_ptr<rats::gait_generator> AutoBalancer::ggPtr [private]

Definition at line 245 of file AutoBalancer.h.

Member Enumeration Documentation

anonymous enum [private]

Enumerator:

BIPED
TROT
PACE
CRAWL
GALLOP

Definition at line 252 of file AutoBalancer.h.

anonymous enum [private]

Enumerator:

MODE_IDLE
MODE_ABC
MODE_SYNC_TO_IDLE
MODE_SYNC_TO_ABC

Definition at line 253 of file AutoBalancer.h.

anonymous enum [private]

Enumerator:

MODE_NO_FORCE
MODE_REF_FORCE
MODE_REF_FORCE_WITH_FOOT
MODE_REF_FORCE_RFU_EXT_MOMENT

Definition at line 278 of file AutoBalancer.h.

Constructor & Destructor Documentation

AutoBalancer::AutoBalancer ( RTC::Manager * manager )

Definition at line 54 of file AutoBalancer.cpp.

AutoBalancer::~AutoBalancer ( ) [virtual]

Definition at line 89 of file AutoBalancer.cpp.

Member Function Documentation

bool AutoBalancer::adjustFootSteps	(	const OpenHRP::AutoBalancerService::Footstep &	rfootstep,
		const OpenHRP::AutoBalancerService::Footstep &	lfootstep
	)

Definition at line 1906 of file AutoBalancer.cpp.

bool AutoBalancer::calc_inital_support_legs	(	const double &	y,
		std::vector< rats::coordinates > &	initial_support_legs_coords,
		std::vector< rats::leg_type > &	initial_support_legs,
		rats::coordinates &	start_ref_coords
	)		`[private]`

Definition at line 2130 of file AutoBalancer.cpp.

void AutoBalancer::calc_static_balance_point_from_forces	(	hrp::Vector3 &	sb_point,
		const hrp::Vector3 &	tmpcog,
		const double	ref_com_height,
		std::vector< hrp::Vector3 > &	tmp_forces
	)		`[private]`

Definition at line 2033 of file AutoBalancer.cpp.

hrp::Vector3 AutoBalancer::calc_vel_from_hand_error ( const rats::coordinates & tmp_fix_coords ) [private]

Definition at line 2090 of file AutoBalancer.cpp.

void AutoBalancer::calcFixCoordsForAdjustFootstep ( rats::coordinates & tmp_fix_coords ) [private]

Definition at line 882 of file AutoBalancer.cpp.

hrp::Vector3 AutoBalancer::calcFootMidPosUsingZMPWeightMap ( ) [private]

Definition at line 981 of file AutoBalancer.cpp.

void AutoBalancer::calcReferenceJointAnglesForIK ( ) [private]

Definition at line 1011 of file AutoBalancer.cpp.

void AutoBalancer::calculateOutputRefForces ( ) [private]

Definition at line 956 of file AutoBalancer.cpp.

void AutoBalancer::copyRatscoords2Footstep	(	OpenHRP::AutoBalancerService::Footstep &	out_fs,
		const rats::coordinates &	in_fs
	)		`[private]`

Definition at line 1875 of file AutoBalancer.cpp.

void AutoBalancer::distributeReferenceZMPToWrenches ( const hrp::Vector3 & _ref_zmp )

Definition at line 2179 of file AutoBalancer.cpp.

bool AutoBalancer::emergencyStop ( )

Definition at line 1316 of file AutoBalancer.cpp.

void AutoBalancer::fixLegToCoords	(	const hrp::Vector3 &	fix_pos,
		const hrp::Matrix33 &	fix_rot
	)		`[private]`

Definition at line 1037 of file AutoBalancer.cpp.

void AutoBalancer::fixLegToCoords2 ( rats::coordinates & tmp_fix_coords ) [private]

Definition at line 1057 of file AutoBalancer.cpp.

bool AutoBalancer::getAutoBalancerParam ( OpenHRP::AutoBalancerService::AutoBalancerParam & i_param )

Definition at line 1794 of file AutoBalancer.cpp.

bool AutoBalancer::getFootstepParam ( OpenHRP::AutoBalancerService::FootstepParam & i_param )

Definition at line 1885 of file AutoBalancer.cpp.

bool AutoBalancer::getGaitGeneratorParam ( OpenHRP::AutoBalancerService::GaitGeneratorParam & i_param )

Definition at line 1554 of file AutoBalancer.cpp.

bool AutoBalancer::getGoPosFootstepsSequence	(	const double &	x,
		const double &	y,
		const double &	th,
		OpenHRP::AutoBalancerService::FootstepsSequence_out	o_footstep
	)

Definition at line 1985 of file AutoBalancer.cpp.

void AutoBalancer::getOutputParametersForABC ( ) [private]

Definition at line 802 of file AutoBalancer.cpp.

void AutoBalancer::getOutputParametersForIDLE ( ) [private]

Definition at line 832 of file AutoBalancer.cpp.

void AutoBalancer::getOutputParametersForWalking ( ) [private]

Definition at line 762 of file AutoBalancer.cpp.

bool AutoBalancer::getRemainingFootstepSequence	(	OpenHRP::AutoBalancerService::FootstepSequence_out	o_footstep,
		CORBA::Long &	o_current_fs_idx
	)

Definition at line 1970 of file AutoBalancer.cpp.

void AutoBalancer::getTargetParameters ( ) [private]

Definition at line 665 of file AutoBalancer.cpp.

std::string AutoBalancer::getUseForceModeString ( ) [private]

Definition at line 2279 of file AutoBalancer.cpp.

bool AutoBalancer::goPos	(	const double &	x,
		const double &	y,
		const double &	th
	)

Definition at line 1245 of file AutoBalancer.cpp.

bool AutoBalancer::goStop ( )

Definition at line 1309 of file AutoBalancer.cpp.

bool AutoBalancer::goVelocity	(	const double &	vx,
		const double &	vy,
		const double &	vth
	)

Definition at line 1273 of file AutoBalancer.cpp.

void AutoBalancer::interpolateLegNamesAndZMPOffsets ( ) [private]

Definition at line 859 of file AutoBalancer.cpp.

bool AutoBalancer::isOptionalDataContact ( const std::string & ee_name ) [inline, private]

Definition at line 237 of file AutoBalancer.h.

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

Reimplemented from RTC::RTObject_impl.

Definition at line 411 of file AutoBalancer.cpp.

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

Reimplemented from RTC::RTObject_impl.

Definition at line 418 of file AutoBalancer.cpp.

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

Reimplemented from RTC::RTObject_impl.

Definition at line 433 of file AutoBalancer.cpp.

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

Reimplemented from RTC::RTObject_impl.

Definition at line 388 of file AutoBalancer.cpp.

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

Reimplemented from RTC::RTObject_impl.

Definition at line 94 of file AutoBalancer.cpp.

hrp::Matrix33 AutoBalancer::OrientRotationMatrix	(	const hrp::Matrix33 &	rot,
		const hrp::Vector3 &	axis1,
		const hrp::Vector3 &	axis2
	)		`[private]`

Definition at line 1026 of file AutoBalancer.cpp.

bool AutoBalancer::releaseEmergencyStop ( )

Definition at line 1325 of file AutoBalancer.cpp.

void AutoBalancer::rotateRefForcesForFixCoords ( rats::coordinates & tmp_fix_coords ) [private]

Definition at line 900 of file AutoBalancer.cpp.

bool AutoBalancer::setAutoBalancerParam ( const OpenHRP::AutoBalancerService::AutoBalancerParam & i_param )

Definition at line 1645 of file AutoBalancer.cpp.

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
	)

Definition at line 1334 of file AutoBalancer.cpp.

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
	)