Skip to content
Snippets Groups Projects
Commit 55f82260 authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[estimator] Use device.velocity instead of robotVelocity

parent d213de46
No related branches found
No related tags found
No related merge requests found
......@@ -120,7 +120,7 @@ def create_imu_offset_compensation(robot, dt):
def create_device_filters(robot, dt):
robot.vselec = Selec_of_vector("vselec")
robot.vselec.selec(6,6+N_JOINTS)
plug(robot.device.robotVelocity,robot.vselec.sin)
plug(robot.device.velocity,robot.vselec.sin)
filters = Bunch();
filters.joints_kin = create_chebi1_checby2_series_filter("joints_kin", dt, N_JOINTS);
......@@ -131,7 +131,6 @@ def create_device_filters(robot, dt):
filters.torque_filter = create_chebi1_checby2_series_filter("ptorque_filter", dt, N_JOINTS);
filters.acc_filter = create_chebi1_checby2_series_filter("acc_filter", dt, 3);
filters.gyro_filter = create_chebi1_checby2_series_filter("gyro_filter", dt, 3);
#filters.estimator_kin = create_chebi1_checby2_series_filter("estimator_kin", dt, N_JOINTS);
filters.vel_filter = create_butter_lp_filter_Wn_04_N_2("vel_filter", dt, N_JOINTS);
plug(robot.device.joint_angles, filters.joints_kin.x);
......@@ -172,9 +171,8 @@ def create_base_estimator(robot, dt, conf, robot_name="robot"):
plug(robot.device_filters.ft_RF_filter.x_filtered, base_estimator.forceRLEG)
plug(robot.device_filters.ft_LF_filter.dx, base_estimator.dforceLLEG)
plug(robot.device_filters.ft_RF_filter.dx, base_estimator.dforceRLEG)
# plug(robot.device_filters.estimator_kin.dx, base_estimator.joint_velocities)
# plug(robot.vselec.sout, base_estimator.joint_velocities)
plug(robot.device_filters.vel_filter.x_filtered, base_estimator.joint_velocities)
plug(robot.vselec.sout, base_estimator.joint_velocities)
# plug(robot.device_filters.vel_filter.x_filtered, base_estimator.joint_velocities)
plug(robot.imu_filters.imu_quat, base_estimator.imu_quaternion)
plug(robot.device_filters.gyro_filter.x_filtered, base_estimator.gyroscope)
plug(robot.device_filters.acc_filter.x_filtered, base_estimator.accelerometer)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment