Commit f29e3871 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

[Python] format

parent d7667c25
[flake8]
max-line-length = 119
exclude = cmake
ignore = N801,N802,N803,N806,N816
......@@ -3,43 +3,26 @@ from time import sleep
import numpy as np
from dynamic_graph import plug
from dynamic_graph.sot.core.madgwickahrs import MadgwickAHRS
from dynamic_graph.sot.core.operator import (Component_of_vector, MatrixHomoToPoseQuaternion,
MatrixHomoToPoseRollPitchYaw, Mix_of_vector, PoseRollPitchYawToMatrixHomo,
Selec_of_vector)
from dynamic_graph.sot.core.operator import Mix_of_vector, Selec_of_vector
from dynamic_graph.sot.core.parameter_server import ParameterServer
from dynamic_graph.sot_talos_balance.admittance_controller_end_effector import AdmittanceControllerEndEffector
from dynamic_graph.sot_talos_balance.ankle_admittance_controller import AnkleAdmittanceController
from dynamic_graph.sot_talos_balance.ankle_joint_selector import AnkleJointSelector
from dynamic_graph.sot_talos_balance.boolean_identity import BooleanIdentity
from dynamic_graph.sot_talos_balance.com_admittance_controller import ComAdmittanceController
from dynamic_graph.sot_talos_balance.dcm_com_controller import DcmComController
from dynamic_graph.sot_talos_balance.dcm_controller import DcmController
from dynamic_graph.sot_talos_balance.dcm_estimator import DcmEstimator
from dynamic_graph.sot_talos_balance.delay import DelayVector
from dynamic_graph.sot_talos_balance.distribute_wrench import DistributeWrench
from dynamic_graph.sot_talos_balance.dummy_dcm_estimator import DummyDcmEstimator
from dynamic_graph.sot_talos_balance.dummy_walking_pattern_generator import DummyWalkingPatternGenerator
from dynamic_graph.sot_talos_balance.euler_to_quat import EulerToQuat
from dynamic_graph.sot_talos_balance.example import Example
from dynamic_graph.sot_talos_balance.foot_force_difference_controller import FootForceDifferenceController
from dynamic_graph.sot_talos_balance.ft_calibration import FtCalibration
from dynamic_graph.sot_talos_balance.ft_wrist_calibration import FtWristCalibration
from dynamic_graph.sot_talos_balance.hip_flexibility_compensation import HipFlexibilityCompensation
from dynamic_graph.sot_talos_balance.int_identity import IntIdentity
from dynamic_graph.sot_talos_balance.joint_position_controller import JointPositionController
from dynamic_graph.sot_talos_balance.nd_trajectory_generator import NdTrajectoryGenerator
from dynamic_graph.sot_talos_balance.pose_quaternion_to_matrix_homo import PoseQuaternionToMatrixHomo
from dynamic_graph.sot_talos_balance.qualisys_client import QualisysClient
from dynamic_graph.sot_talos_balance.quat_to_euler import QuatToEuler
from dynamic_graph.sot_talos_balance.round_double_to_int import RoundDoubleToInt
from dynamic_graph.sot_talos_balance.simple_admittance_controller import SimpleAdmittanceController
from dynamic_graph.sot_talos_balance.simple_distribute_wrench import SimpleDistributeWrench
from dynamic_graph.sot_talos_balance.simple_pid import SimplePID
from dynamic_graph.sot_talos_balance.simple_pidd import SimplePIDD
from dynamic_graph.sot_talos_balance.simple_reference_frame import SimpleReferenceFrame
from dynamic_graph.sot_talos_balance.simple_state_integrator import SimpleStateIntegrator
from dynamic_graph.sot_talos_balance.simple_zmp_estimator import SimpleZmpEstimator
from dynamic_graph.sot_talos_balance.state_transformation import StateTransformation
from dynamic_graph.sot_talos_balance.talos_base_estimator import TalosBaseEstimator
from dynamic_graph.sot_talos_balance.talos_control_manager import TalosControlManager
# python
......
import subprocess
import yaml
import rosbag
import rospy
import yaml
bagname = '2017-07-24-08-04-30.bag'
......@@ -64,9 +65,11 @@ def extract_data():
diff_nanosecs = 1000000000 + diff_nanosecs
diff_secs = diff_secs - 1
afile.write(
str(messages[jn].motorenc) + ' ' + str(messages[jn].jointenc) + ' ' + str(messages[jn].torquesensor) +
' ' + str(messages[jn].current) + ' ' + str(diff_secs) + '.' + str(diff_nanosecs).zfill(9) + '\n')
afile.write(' '.join(
str(m) for m in [
messages[jn].motorenc, messages[jn].jointenc, messages[jn].torquesensor, messages[jn].current,
diff_secs
]) + '.' + str(diff_nanosecs).zfill(9) + '\n')
for jn in jointnames:
files[jn].close()
......@@ -83,11 +86,12 @@ def extract_batteries_data():
diff_nanosecs = 1000000000 + diff_nanosecs
diff_secs = diff_secs - 1
f.write(
str(aps.message.charge) + ' ' + str(aps.message.current) + ' ' + str(aps.message.input_voltage) + ' ' +
str(aps.message.battery_voltage) + ' ' + str(aps.message.regulator_voltage) + ' ' +
str(aps.message.lower_body_voltage) + ' ' + str(aps.message.charger_voltage) + ' ' +
str(aps.message.upper_body_voltage) + ' ' + str(diff_secs) + '.' + str(diff_nanosecs).zfill(9) + '\n')
f.write(' '.join(
str(m) for m in [
aps.message.charge, aps.message.current, aps.message.input_voltage, aps.message.battery_voltage,
aps.message.regulator_voltage, aps.message.lower_body_voltage, aps.message.charger_voltage,
aps.message.upper_body_voltage, diff_secs
]) + '.' + str(diff_nanosecs).zfill(9) + '\n')
f.close()
......
......@@ -97,5 +97,5 @@ footFrameNames = {"Right": "leg_right_6_joint", "Left": "leg_left_6_joint"}
rightFootSensorXYZ = (0.0, 0.0, -0.085)
rightFootSoleXYZ = (0.0, 0.0, -0.105)
urdftosot = (0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18,\
19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31)
urdftosot = (0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28,
29, 30, 31)
......@@ -130,8 +130,10 @@ create_topic(robot.publisher, robot.controller, 'force', robot=robot, data_type=
create_topic(robot.publisher, robot.controller, 'dq', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.controller, 'w_dq', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.controller, 'w_forceDes', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.forceCalibrator, 'leftWristForceOut', robot=robot, data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.forceCalibrator, 'rightWristForceOut', robot=robot, data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.forceCalibrator, 'leftWristForceOut', robot=robot,
data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.forceCalibrator, 'rightWristForceOut', robot=robot,
data_type='vector') # calibrated right wrench
# # --- ROS SUBSCRIBER
robot.subscriber = RosSubscribe("end_effector_subscriber")
......
......@@ -347,8 +347,10 @@ create_topic(robot.publisher, robot.zmp_estimator, 'zmp', robot=robot, data_type
create_topic(robot.publisher, robot.wrenchDistributor, 'wrenchLeft', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.wrenchDistributor, 'wrenchRight', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot, data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot, data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot,
data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot,
data_type='vector') # calibrated right wrench
# --- TRACER
robot.tracer = TracerRealTime("com_tracer")
......
......@@ -473,10 +473,13 @@ create_topic(robot.publisher, robot.dynamic, 'zmp', robot=robot, data_type='vect
create_topic(robot.publisher, robot.zmp_estimator, 'zmp', robot=robot, data_type='vector') # estimated ZMP
create_topic(robot.publisher, robot.dcm_control, 'zmpRef', robot=robot, data_type='vector') # reference ZMP
create_topic(robot.publisher, robot.dcm_control, 'wrenchRef', robot=robot, data_type='vector') # unoptimized reference wrench
create_topic(robot.publisher, robot.dcm_control, 'wrenchRef', robot=robot,
data_type='vector') # unoptimized reference wrench
create_topic(robot.publisher, robot.distribute, 'wrenchLeft', robot=robot, data_type='vector') # reference left wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot=robot, data_type='vector') # reference right wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot, data_type='vector') # optimized reference wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot=robot,
data_type='vector') # reference right wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot,
data_type='vector') # optimized reference wrench
#create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector') # measured left wrench
#create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector') # measured right wrench
......@@ -484,8 +487,10 @@ create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot, data_t
#create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
#create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot, data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot, data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot,
data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot,
data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.rightPitchAnkleController, 'tau', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.rightPitchAnkleController, 'tauDes', robot=robot, data_type='vector')
......
......@@ -358,10 +358,13 @@ create_topic(robot.publisher, robot.dynamic, 'zmp', robot=robot, data_type='vect
create_topic(robot.publisher, robot.zmp_estimator, 'zmp', robot=robot, data_type='vector') # estimated ZMP
create_topic(robot.publisher, robot.dcm_control, 'zmpRef', robot=robot, data_type='vector') # reference ZMP
create_topic(robot.publisher, robot.dcm_control, 'wrenchRef', robot=robot, data_type='vector') # unoptimized reference wrench
create_topic(robot.publisher, robot.dcm_control, 'wrenchRef', robot=robot,
data_type='vector') # unoptimized reference wrench
create_topic(robot.publisher, robot.distribute, 'wrenchLeft', robot=robot, data_type='vector') # reference left wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot=robot, data_type='vector') # reference right wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot, data_type='vector') # optimized reference wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot=robot,
data_type='vector') # reference right wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot,
data_type='vector') # optimized reference wrench
#create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector') # measured left wrench
#create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector') # measured right wrench
......@@ -369,8 +372,10 @@ create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot, data_t
#create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
#create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot, data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot, data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot,
data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot,
data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.zmp_estimator, 'copRight', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.rightAnkleController, 'pRef', robot=robot, data_type='vector')
......
......@@ -487,13 +487,18 @@ create_topic(robot.publisher, robot.dynamic, 'zmp', robot=robot, data_type='vect
create_topic(robot.publisher, robot.zmp_estimator, 'zmp', robot=robot, data_type='vector') # estimated ZMP
create_topic(robot.publisher, robot.dcm_control, 'zmpRef', robot=robot, data_type='vector') # reference ZMP
create_topic(robot.publisher, robot.dcm_control, 'wrenchRef', robot=robot, data_type='vector') # unoptimized reference wrench
create_topic(robot.publisher, robot.dcm_control, 'wrenchRef', robot=robot,
data_type='vector') # unoptimized reference wrench
create_topic(robot.publisher, robot.distribute, 'wrenchLeft', robot=robot, data_type='vector') # reference left wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot=robot, data_type='vector') # reference right wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot, data_type='vector') # optimized reference wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot, data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot, data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot=robot,
data_type='vector') # reference right wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot,
data_type='vector') # optimized reference wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot,
data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot,
data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.pitchController, 'tauL', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.pitchController, 'tauDesL', robot=robot, data_type='vector')
......
......@@ -401,10 +401,13 @@ create_topic(robot.publisher, robot.dynamic, 'zmp', robot=robot, data_type='vect
create_topic(robot.publisher, robot.zmp_estimator, 'zmp', robot=robot, data_type='vector') # estimated ZMP
create_topic(robot.publisher, robot.dcm_control, 'zmpRef', robot=robot, data_type='vector') # reference ZMP
create_topic(robot.publisher, robot.dcm_control, 'wrenchRef', robot=robot, data_type='vector') # unoptimized reference wrench
create_topic(robot.publisher, robot.dcm_control, 'wrenchRef', robot=robot,
data_type='vector') # unoptimized reference wrench
create_topic(robot.publisher, robot.distribute, 'wrenchLeft', robot=robot, data_type='vector') # reference left wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot=robot, data_type='vector') # reference right wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot, data_type='vector') # optimized reference wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot=robot,
data_type='vector') # reference right wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot,
data_type='vector') # optimized reference wrench
#create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector') # measured left wrench
#create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector') # measured right wrench
......@@ -412,8 +415,10 @@ create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot, data_t
#create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
#create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot, data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot, data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot,
data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot,
data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.rightPitchAnkleController, 'tau', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.rightPitchAnkleController, 'tauDes', robot=robot, data_type='vector')
......
......@@ -362,10 +362,13 @@ create_topic(robot.publisher, robot.dynamic, 'zmp', robot=robot, data_type='vect
create_topic(robot.publisher, robot.zmp_estimator, 'zmp', robot=robot, data_type='vector') # estimated ZMP
create_topic(robot.publisher, robot.dcm_control, 'zmpRef', robot=robot, data_type='vector') # reference ZMP
create_topic(robot.publisher, robot.dcm_control, 'wrenchRef', robot=robot, data_type='vector') # unoptimized reference wrench
create_topic(robot.publisher, robot.dcm_control, 'wrenchRef', robot=robot,
data_type='vector') # unoptimized reference wrench
create_topic(robot.publisher, robot.distribute, 'wrenchLeft', robot=robot, data_type='vector') # reference left wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot=robot, data_type='vector') # reference right wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot, data_type='vector') # optimized reference wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot=robot,
data_type='vector') # reference right wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot,
data_type='vector') # optimized reference wrench
#create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector') # measured left wrench
#create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector') # measured right wrench
......@@ -373,8 +376,10 @@ create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot, data_t
#create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
#create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot, data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot, data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot,
data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot,
data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.zmp_estimator, 'copRight', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.rightAnkleController, 'pRef', robot=robot, data_type='vector')
......
......@@ -411,10 +411,13 @@ create_topic(robot.publisher, robot.dynamic, 'zmp', robot=robot, data_type='vect
create_topic(robot.publisher, robot.zmp_estimator, 'zmp', robot=robot, data_type='vector') # estimated ZMP
create_topic(robot.publisher, robot.dcm_control, 'zmpRef', robot=robot, data_type='vector') # reference ZMP
create_topic(robot.publisher, robot.dcm_control, 'wrenchRef', robot=robot, data_type='vector') # unoptimized reference wrench
create_topic(robot.publisher, robot.dcm_control, 'wrenchRef', robot=robot,
data_type='vector') # unoptimized reference wrench
create_topic(robot.publisher, robot.distribute, 'wrenchLeft', robot=robot, data_type='vector') # reference left wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot=robot, data_type='vector') # reference right wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot, data_type='vector') # optimized reference wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot=robot,
data_type='vector') # reference right wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot,
data_type='vector') # optimized reference wrench
#create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector') # measured left wrench
#create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector') # measured right wrench
......@@ -422,8 +425,10 @@ create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot, data_t
#create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
#create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot, data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot, data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot,
data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot,
data_type='vector') # calibrated right wrench
# --- TRACER
robot.tracer = TracerRealTime("com_tracer")
......
......@@ -389,8 +389,10 @@ create_topic(robot.publisher, robot.dcm_control, 'zmpRef', robot=robot, data_typ
#create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
#create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot, data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot, data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot,
data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot,
data_type='vector') # calibrated right wrench
# --- TRACER
robot.tracer = TracerRealTime("com_tracer")
......
......@@ -325,8 +325,10 @@ create_topic(robot.publisher, robot.dcm_control, 'zmpRef', robot=robot, data_typ
#create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
#create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot, data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot, data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot,
data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot,
data_type='vector') # calibrated right wrench
# --- TRACER
robot.tracer = TracerRealTime("zmp_tracer")
......
......@@ -31,7 +31,7 @@ g = 9.81
omega = sqrt(g / h)
# --- Parameter server
fill_parameter_server(robot.param_server,param_server_conf, dt)
fill_parameter_server(robot.param_server, param_server_conf, dt)
# --- Initial feet and waist
robot.dynamic.createOpPoint('LF', robot.OperationalPointsMap['left-ankle'])
......@@ -435,8 +435,10 @@ create_topic(robot.publisher, robot.waistTrajGen, 'x', robot=robot, data_type='v
create_topic(robot.publisher, robot.lfTrajGen, 'x', robot=robot, data_type='vector') # desired left foot pose
create_topic(robot.publisher, robot.rfTrajGen, 'x', robot=robot, data_type='vector') # desired right foot pose
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot, data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot, data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot,
data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot,
data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.dynamic, 'LF', robot=robot, data_type='matrixHomo') # left foot
create_topic(robot.publisher, robot.dynamic, 'RF', robot=robot, data_type='matrixHomo') # right foot
......
......@@ -385,10 +385,13 @@ create_topic(robot.publisher, robot.dynamic, 'zmp', robot=robot, data_type='vect
create_topic(robot.publisher, robot.zmp_estimator, 'zmp', robot=robot, data_type='vector') # estimated ZMP
create_topic(robot.publisher, robot.dcm_control, 'zmpRef', robot=robot, data_type='vector') # reference ZMP
create_topic(robot.publisher, robot.dcm_control, 'wrenchRef', robot=robot, data_type='vector') # unoptimized reference wrench
create_topic(robot.publisher, robot.dcm_control, 'wrenchRef', robot=robot,
data_type='vector') # unoptimized reference wrench
create_topic(robot.publisher, robot.distribute, 'wrenchLeft', robot=robot, data_type='vector') # reference left wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot=robot, data_type='vector') # reference right wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot, data_type='vector') # optimized reference wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot=robot,
data_type='vector') # reference right wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot,
data_type='vector') # optimized reference wrench
#create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector') # measured left wrench
#create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector') # measured right wrench
......@@ -396,8 +399,10 @@ create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot, data_t
#create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
#create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot, data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot, data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot,
data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot,
data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.zmp_estimator, 'copRight', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.rightAnkleController, 'pRef', robot=robot, data_type='vector')
......
......@@ -400,12 +400,17 @@ create_topic(robot.publisher, robot.dynamic, 'zmp', robot=robot, data_type='vect
create_topic(robot.publisher, robot.zmp_estimator, 'zmp', robot=robot, data_type='vector') # estimated ZMP
create_topic(robot.publisher, robot.dcm_control, 'zmpRef', robot=robot, data_type='vector') # reference ZMP
create_topic(robot.publisher, robot.dcm_control, 'wrenchRef', robot=robot, data_type='vector') # unoptimized reference wrench
create_topic(robot.publisher, robot.dcm_control, 'wrenchRef', robot=robot,
data_type='vector') # unoptimized reference wrench
create_topic(robot.publisher, robot.distribute, 'wrenchLeft', robot=robot, data_type='vector') # reference left wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot=robot, data_type='vector') # reference right wrench
create_topic(robot.publisher, robot.distribute, 'surfaceWrenchLeft', robot=robot, data_type='vector') # reference surface left wrench
create_topic(robot.publisher, robot.distribute, 'surfaceWrenchRight', robot=robot, data_type='vector') # reference right wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot, data_type='vector') # optimized reference wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot=robot,
data_type='vector') # reference right wrench
create_topic(robot.publisher, robot.distribute, 'surfaceWrenchLeft', robot=robot,
data_type='vector') # reference surface left wrench
create_topic(robot.publisher, robot.distribute, 'surfaceWrenchRight', robot=robot,
data_type='vector') # reference right wrench
create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot,
data_type='vector') # optimized reference wrench
#create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector') # measured left wrench
#create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector') # measured right wrench
......@@ -413,8 +418,10 @@ create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot=robot, data_t
#create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
#create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot, data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot, data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot,
data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot,
data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.zmp_estimator, 'copRight', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.zmp_estimator, 'copLeft', robot=robot, data_type='vector')
......
......@@ -347,7 +347,8 @@ create_topic(robot.publisher, robot.dynamic, 'zmp', robot=robot, data_type='vect
create_topic(robot.publisher, robot.zmp_estimator, 'zmp', robot=robot, data_type='vector') # estimated ZMP
create_topic(robot.publisher, robot.dcm_control, 'zmpRef', robot=robot, data_type='vector') # reference ZMP
create_topic(robot.publisher, robot.dcm_control, 'wrenchRef', robot=robot, data_type='vector') # unoptimized reference wrench
create_topic(robot.publisher, robot.dcm_control, 'wrenchRef', robot=robot,
data_type='vector') # unoptimized reference wrench
#create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector') # measured left wrench
#create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector') # measured right wrench
......@@ -355,8 +356,10 @@ create_topic(robot.publisher, robot.dcm_control, 'wrenchRef', robot=robot, data_
#create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
#create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot, data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot, data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot,
data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot,
data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.zmp_estimator, 'copRight', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.zmp_estimator, 'copLeft', robot=robot, data_type='vector')
......
......@@ -140,7 +140,8 @@ create_topic(robot.publisher, robot.cdc_estimator, 'dc', robot=robot, data_type=
create_topic(robot.publisher, robot.estimator, 'dcm', robot=robot, data_type='vector') # estimated DCM
create_topic(robot.publisher, robot.zmp_estimator, 'zmp', robot=robot, data_type='vector') # estimated ZMP
create_topic(robot.publisher, robot.zmp_estimator, 'emergencyStop', robot=robot, data_type='boolean') # ZMP emergency stop
create_topic(robot.publisher, robot.zmp_estimator, 'emergencyStop', robot=robot,
data_type='boolean') # ZMP emergency stop
create_topic(robot.publisher, robot.dynamic, 'com', robot=robot, data_type='vector') # SOT CoM
create_topic(robot.publisher, robot.dynamic, 'zmp', robot=robot, data_type='vector') # SOT ZMP
create_topic(robot.publisher, robot.device, 'forceLLEG', robot=robot, data_type='vector') # force on left foot
......@@ -152,5 +153,7 @@ create_topic(robot.publisher, robot.device, 'forceRLEG', robot=robot, data_type=
#create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
#create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot, data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot, data_type='vector') # calibrated right wrench
create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot,
data_type='vector') # calibrated left wrench
create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot,
data_type='vector') # calibrated right wrench
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment