def test_velocity_filters(robot):
conf_traj = conf_velocity_filter()
conf_list = conf_filter_list()
filter_b = tuple(conf_list.b_list[0])
filter_a = tuple(conf_list.a_list[0])
b_list = [tuple(b) for b in conf_list.b_list[1:]]
a_list = [tuple(a) for a in conf_list.a_list[1:]]
if SIM_MODE:
conf = get_sim_conf()
else:
conf = get_default_conf()
setup_velocity_filter(robot, conf, filter_b, filter_a)
print "Gonna start SoT"
sleep(1.0)
start_sot()
print "Gonna go to half sitting"
sleep(1.0)
wait_for_motion()
go_to_position(robot.traj_gen, robot.halfSitting[6:], 10.0)
wait_for_motion()
index = 0
dir_name = '/tmp/' + conf_traj.j_name + '/filter_0/'
robot.tracer = create_tracer(robot, dir_name)
robot.tracer.start()
robot.traj_gen.startLinChirp(conf_traj.j_name, conf_traj.x_f,
conf_traj.w_min, conf_traj.w_max,
conf_traj.deltaT)
wait_for_motion()
dump_stop_tracer(robot.tracer)
print "Gonna go to half sitting"
sys.__stdout__.flush()
sleep(1.0)
go_to_position(robot.traj_gen, robot.halfSitting[6:], 10.0)
for b, a in zip(b_list, a_list):
index += 1
dir_name = '/tmp/' + conf_traj.j_name + '/filter_' + str(index) + '/'
robot.estimator_fd.switch_filter(b, a)
robot.tracer = create_tracer(robot, dir_name)
print "Gonna start chirp"
sys.__stdout__.flush()
robot.tracer.start()
robot.traj_gen.startLinChirp(conf_traj.j_name, conf_traj.x_f,
conf_traj.w_min, conf_traj.w_max,
conf_traj.deltaT)
wait_for_motion()
dump_stop_tracer(robot.tracer)
print "Gonna go to half sitting"
sys.__stdout__.flush()
sleep(1.0)
go_to_position(robot.traj_gen, robot.halfSitting[6:], 10.0)
wait_for_motion()
def test_balance_ctrl_openhrp(robot,
use_real_vel=True,
use_real_base_state=False,
startSoT=True):
# BUILD THE STANDARD GRAPH
conf = get_sim_conf()
robot = main_v3(robot, startSoT=False, go_half_sitting=False, conf=conf)
# force current measurements to zero
robot.ctrl_manager.i_measured.value = NJ * (0.0, )
robot.current_ctrl.i_measured.value = NJ * (0.0, )
robot.filters.current_filter.x.value = NJ * (0.0, )
# BYPASS TORQUE CONTROLLER
plug(robot.inv_dyn.tau_des, robot.ctrl_manager.ctrl_torque)
# CREATE SIGNALS WITH ROBOT STATE WITH CORRECT SIZE (36)
robot.q = Selec_of_vector("q")
plug(robot.device.robotState, robot.q.sin)
robot.q.selec(0, NJ + 6)
plug(robot.q.sout, robot.pos_ctrl.base6d_encoders)
plug(robot.q.sout, robot.traj_gen.base6d_encoders)
plug(robot.q.sout, robot.estimator_ft.base6d_encoders)
robot.ros = RosPublish('rosPublish')
robot.device.after.addDownsampledSignal('rosPublish.trigger', 1)
# BYPASS JOINT VELOCITY ESTIMATOR
if (use_real_vel):
robot.dq = Selec_of_vector("dq")
plug(robot.device.robotVelocity, robot.dq.sin)
robot.dq.selec(6, NJ + 6)
plug(robot.dq.sout, robot.pos_ctrl.jointsVelocities)
plug(robot.dq.sout, robot.base_estimator.joint_velocities)
plug(robot.device.gyrometer, robot.base_estimator.gyroscope)
# BYPASS BASE ESTIMATOR
robot.v = Selec_of_vector("v")
plug(robot.device.robotVelocity, robot.v.sin)
robot.v.selec(0, NJ + 6)
if (use_real_base_state):
plug(robot.q.sout, robot.inv_dyn.q)
plug(robot.v.sout, robot.inv_dyn.v)
if (startSoT):
start_sot()
# RESET FORCE/TORQUE SENSOR OFFSET
sleep(10 * robot.timeStep)
robot.estimator_ft.setFTsensorOffsets(24 * (0.0, ))
return robot
def main_v2(
robot,
delay=0.01,
startSoT=True,
go_half_sitting=True,
urdfFileName='/opt/openrobots/share/hrp2_14_description/urdf/hrp2_14.urdf'
):
dt = robot.timeStep
robot.device.setControlInputType('position')
robot.traj_gen = create_trajectory_generator(robot.device, dt)
robot.com_traj_gen = create_com_traj_gen(dt)
robot.rf_traj_gen = SE3TrajectoryGenerator("tg_rf")
robot.lf_traj_gen = SE3TrajectoryGenerator("tg_lf")
robot.rf_traj_gen.init(dt)
robot.lf_traj_gen.init(dt)
(robot.estimator_ft,
robot.estimator_kin) = create_estimators(robot, dt, delay)
robot.ff_locator = create_free_flyer_locator(robot, urdfFileName)
robot.flex_est = create_flex_estimator(robot, dt)
robot.floatingBase = create_floatingBase(robot)
robot.pos_ctrl = create_position_controller(robot, dt)
robot.torque_ctrl = create_torque_controller(robot)
# inv_dyn = create_inverse_dynamics(robot, dt);
robot.inv_dyn = create_balance_controller(robot, urdfFileName, dt)
robot.ctrl_manager = create_ctrl_manager(robot, dt)
robot.estimator_ft.gyroscope.value = (0.0, 0.0, 0.0)
# estimator.accelerometer.value = (0.0, 0.0, 9.81);
if (startSoT):
print "Gonna start SoT"
sleep(1.0)
start_sot()
if (go_half_sitting):
print "Gonna go to half sitting"
sleep(1.0)
go_to_position(robot.traj_gen, robot.halfSitting[6:], 10.0)
return robot
def main_v3(robot, startSoT=True, go_half_sitting=True, conf=None):
if (conf is None):
conf = get_default_conf()
dt = robot.timeStep
# TMP: overwrite halfSitting configuration to use SoT joint order
robot.halfSitting = (
# Free flyer
0.,
0.,
0.648702,
0.,
0.,
0.,
# Legs
0.,
0.,
-0.453786,
0.872665,
-0.418879,
0.,
0.,
0.,
-0.453786,
0.872665,
-0.418879,
0.,
# Chest and head
0.,
0.,
0.,
0.,
# Arms
0.261799,
-0.17453,
0.,
-0.523599,
0.,
0.,
0.1,
0.261799,
0.17453,
0.,
-0.523599,
0.,
0.,
0.1)
robot.device.setControlInputType('noInteg')
robot.ctrl_manager = create_ctrl_manager(conf.control_manager,
conf.motor_params, dt)
robot.traj_gen = create_trajectory_generator(robot.device, dt)
robot.com_traj_gen = create_com_traj_gen(conf.balance_ctrl, dt)
robot.rf_force_traj_gen = create_force_traj_gen(
"rf_force_ref", conf.balance_ctrl.RF_FORCE_DES, dt)
robot.lf_force_traj_gen = create_force_traj_gen(
"lf_force_ref", conf.balance_ctrl.LF_FORCE_DES, dt)
robot.traj_sync = create_trajectory_switch()
robot.rf_traj_gen = SE3TrajectoryGenerator("tg_rf")
robot.lf_traj_gen = SE3TrajectoryGenerator("tg_lf")
robot.rf_traj_gen.init(dt)
robot.lf_traj_gen.init(dt)
robot.encoders = create_encoders(robot)
robot.imu_offset_compensation = create_imu_offset_compensation(robot, dt)
(robot.estimator_ft,
robot.filters) = create_estimators(robot, conf.force_torque_estimator,
conf.motor_params, dt)
robot.imu_filter = create_imu_filter(robot, dt)
robot.base_estimator = create_base_estimator(robot, dt,
conf.base_estimator)
connect_synchronous_trajectories(robot.traj_sync, [
robot.com_traj_gen, robot.rf_force_traj_gen, robot.lf_force_traj_gen,
robot.rf_traj_gen, robot.lf_traj_gen
])
#robot.rf_traj_gen, robot.lf_traj_gen])
robot.pos_ctrl = create_position_controller(robot, conf.pos_ctrl_gains, dt)
robot.torque_ctrl = create_torque_controller(robot,
conf.joint_torque_controller,
conf.motor_params, dt)
robot.inv_dyn = create_balance_controller(robot, conf.balance_ctrl,
conf.motor_params, dt)
robot.adm_ctrl = create_admittance_ctrl(robot, conf.adm_ctrl, dt)
robot.current_ctrl = create_current_controller(robot, conf.current_ctrl,
conf.motor_params, dt)
connect_ctrl_manager(robot)
# create low-pass filter for computing joint velocities
robot.encoder_filter = create_chebi2_lp_filter_Wn_03_N_4(
'encoder_filter', dt, conf.motor_params.NJ)
plug(robot.encoders.sout, robot.encoder_filter.x)
plug(robot.encoder_filter.dx, robot.current_ctrl.dq)
plug(robot.encoder_filter.dx, robot.torque_ctrl.jointsVelocities)
#plug(robot.encoder_filter.x_filtered, robot.base_estimator.joint_positions);
#plug(robot.encoder_filter.dx, robot.base_estimator.joint_velocities);
robot.ros = RosPublish('rosPublish')
robot.device.after.addDownsampledSignal('rosPublish.trigger', 1)
robot.estimator_ft.dgyro.value = (0.0, 0.0, 0.0)
robot.estimator_ft.gyro.value = (0.0, 0.0, 0.0)
# estimator.accelerometer.value = (0.0, 0.0, 9.81);
if (startSoT):
print "Gonna start SoT"
sleep(1.0)
start_sot()
if (go_half_sitting):
print "Gonna go to half sitting in 1 sec"
sleep(1.0)
go_to_position(robot.traj_gen, robot.halfSitting[6:], 10.0)
return robot