def create_torque_controller(robot, conf, motor_params, dt=0.001, robot_name="robot"):
torque_ctrl = JointTorqueController("jtc");
plug(robot.device.robotState, torque_ctrl.base6d_encoders);
plug(robot.estimator_kin.dx, torque_ctrl.jointsVelocities);
plug(robot.estimator_kin.ddx, torque_ctrl.jointsAccelerations);
plug(robot.estimator_ft.jointsTorques, torque_ctrl.jointsTorques);
plug(robot.estimator_ft.currentFiltered, torque_ctrl.measuredCurrent);
torque_ctrl.jointsTorquesDesired.value = NJ*(0.0,);
torque_ctrl.KpTorque.value = tuple(conf.k_p_torque);
torque_ctrl.KiTorque.value = NJ*(0.0,);
torque_ctrl.KpCurrent.value = tuple(conf.k_p_current);
torque_ctrl.KiCurrent.value = NJ*(0.0,);
torque_ctrl.k_tau.value = tuple(conf.k_tau);
torque_ctrl.k_v.value = tuple(conf.k_v);
torque_ctrl.frictionCompensationPercentage.value = NJ*(conf.FRICTION_COMPENSATION_PERCENTAGE,);
torque_ctrl.motorParameterKt_p.value = tuple(motor_params.Kt_p)
torque_ctrl.motorParameterKt_n.value = tuple(motor_params.Kt_n)
torque_ctrl.motorParameterKf_p.value = tuple(motor_params.Kf_p)
torque_ctrl.motorParameterKf_n.value = tuple(motor_params.Kf_n)
torque_ctrl.motorParameterKv_p.value = tuple(motor_params.Kv_p)
torque_ctrl.motorParameterKv_n.value = tuple(motor_params.Kv_n)
torque_ctrl.motorParameterKa_p.value = tuple(motor_params.Ka_p)
torque_ctrl.motorParameterKa_n.value = tuple(motor_params.Ka_n)
torque_ctrl.polySignDq.value = NJ*(3,);
torque_ctrl.init(dt, robot_name);
return torque_ctrl;
def create_torque_controller(robot,
conf,
motor_params,
dt=0.001,
robot_name="robot"):
torque_ctrl = JointTorqueController("jtc")
plug(robot.encoders.sout, torque_ctrl.jointsPositions)
plug(robot.filters.estimator_kin.dx, torque_ctrl.jointsVelocities)
plug(robot.filters.estimator_kin.ddx, torque_ctrl.jointsAccelerations)
torque_ctrl.jointsTorquesDesired.value = NJ * (0.0, )
torque_ctrl.jointsTorquesDerivative.value = NJ * (0.0, )
torque_ctrl.dq_des.value = NJ * (0.0, )
torque_ctrl.KpTorque.value = tuple(conf.k_p_torque)
torque_ctrl.KdTorque.value = tuple(conf.k_d_torque)
torque_ctrl.KiTorque.value = tuple(conf.k_i_torque)
torque_ctrl.KdVel.value = tuple(conf.k_d_vel)
torque_ctrl.KiVel.value = tuple(conf.k_i_vel)
torque_ctrl.torque_integral_saturation.value = tuple(
conf.torque_integral_saturation)
torque_ctrl.coulomb_friction_compensation_percentage.value = NJ * (
conf.COULOMB_FRICTION_COMPENSATION_PERCENTAGE, )
torque_ctrl.motorParameterKt_p.value = tuple(motor_params.Kt_p)
torque_ctrl.motorParameterKt_n.value = tuple(motor_params.Kt_n)
torque_ctrl.motorParameterKf_p.value = tuple(motor_params.Kf_p)
torque_ctrl.motorParameterKf_n.value = tuple(motor_params.Kf_n)
torque_ctrl.motorParameterKv_p.value = tuple(motor_params.Kv_p)
torque_ctrl.motorParameterKv_n.value = tuple(motor_params.Kv_n)
torque_ctrl.motorParameterKa_p.value = tuple(motor_params.Ka_p)
torque_ctrl.motorParameterKa_n.value = tuple(motor_params.Ka_n)
torque_ctrl.polySignDq.value = NJ * (conf.poly_sign_dq, )
torque_ctrl.init(dt, robot_name)
return torque_ctrl
def create_torque_controller(device, estimator, dt=0.001):
torque_ctrl = JointTorqueController("jtc")
plug(device.robotState, torque_ctrl.base6d_encoders)
plug(estimator.jointsVelocities, torque_ctrl.jointsVelocities)
plug(estimator.jointsAccelerations, torque_ctrl.jointsAccelerations)
plug(estimator.jointsTorques, torque_ctrl.jointsTorques)
plug(estimator.currentFiltered, torque_ctrl.measuredCurrent)
torque_ctrl.jointsTorquesDesired.value = NJ * (0.0, )
torque_ctrl.KpTorque.value = tuple(k_p_torque)
torque_ctrl.KiTorque.value = NJ * (0.0, )
torque_ctrl.KpCurrent.value = tuple(k_p_current)
torque_ctrl.KiCurrent.value = NJ * (0.0, )
torque_ctrl.k_tau.value = tuple(k_tau)
torque_ctrl.k_v.value = tuple(k_v)
torque_ctrl.frictionCompensationPercentage.value = NJ * (0.8, )
# 80%
torque_ctrl.motorParameterKt_p.value = tuple(Kt_p)
torque_ctrl.motorParameterKt_n.value = tuple(Kt_n)
torque_ctrl.motorParameterKf_p.value = tuple(Kf_p)
torque_ctrl.motorParameterKf_n.value = tuple(Kf_n)
torque_ctrl.motorParameterKv_p.value = tuple(Kv_p)
torque_ctrl.motorParameterKv_n.value = tuple(Kv_n)
torque_ctrl.motorParameterKa_p.value = tuple(Ka_p)
torque_ctrl.motorParameterKa_n.value = tuple(Ka_n)
torque_ctrl.polySignDq.value = NJ * (3, )
torque_ctrl.init(dt)
return torque_ctrl