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