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 create_waist_traj_gen(name, robot, dt):
waist_traj_gen = SE3TrajectoryGenerator(name)
ref_waist = robot.dynamic.data.oMi[robot.dynamic.model.getJointId(
'root_joint')]
trans = ref_waist.translation
rot = ref_waist.rotation
rot = rot.reshape(9)
initial_value = np.concatenate((trans, rot))
waist_traj_gen.initial_value.value = tuple(initial_value)
waist_traj_gen.trigger.value = 1.0
waist_traj_gen.init(dt)
return waist_traj_gen
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
from dynamic_graph.sot.torque_control.se3_trajectory_generator import SE3TrajectoryGenerator
from dynamic_graph.sot.torque_control.tests.robot_data_test import initRobotData
# Instanciate a pose trajectory generator
initial_value = (1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0.)
se3tg = SE3TrajectoryGenerator("se3tg_test")
se3tg.init(initRobotData.controlDT)
se3tg.initial_value.value = initial_value
se3tg.trigger.value = 1
se3tg.x.recompute(10)
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., 1.018213, 0., 0. , 0.,
# legs
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708,
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708,
# Chest
0.0 , 0.006761,
# arms
0.25847 , 0.173046, -0.0002, -0.525366, 0.0, -0.0, 0.1, -0.005,
-0.25847 , -0.173046, 0.0002 , -0.525366, 0.0, 0.0, 0.1,-0.005,
# Head
0.0,0.0);
#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.filters = create_filters(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.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;