def create_ctrl_manager(device,
torque_ctrl,
pos_ctrl,
inv_dyn,
estimator,
dt=0.001):
ctrl_manager = ControlManager("ctrl_man")
plug(device.robotState, ctrl_manager.base6d_encoders)
plug(torque_ctrl.predictedJointsTorques, ctrl_manager.tau_predicted)
plug(estimator.jointsTorques, ctrl_manager.tau)
ctrl_manager.max_tau.value = tuple(tau_max)
ctrl_manager.percentageDriverDeadZoneCompensation.value = NJ * (0.5, )
ctrl_manager.signWindowsFilterSize.value = NJ * (2, )
ctrl_manager.bemfFactor.value = NJ * (0.0, )
#ctrl_manager.bemfFactor.value = tuple(Kpwm*0.1);
plug(ctrl_manager.pwmDesSafe, device.control)
plug(ctrl_manager.pwmDes, torque_ctrl.pwm)
ctrl_manager.addCtrlMode("pos")
ctrl_manager.addCtrlMode("torque")
plug(estimator.jointsVelocities, ctrl_manager.dq)
plug(torque_ctrl.controlCurrent, ctrl_manager.ctrl_torque)
plug(pos_ctrl.pwmDes, ctrl_manager.ctrl_pos)
plug(ctrl_manager.joints_ctrl_mode_torque, inv_dyn.active_joints)
ctrl_manager.setCtrlMode("all", "pos")
ctrl_manager.init(dt)
return ctrl_manager
from dynamic_graph.sot.torque_control.control_manager import ControlManager
from dynamic_graph.sot.torque_control.tests.robot_data_test import initRobotData
from numpy import ones, zeros
# Instanciate the free flyer
cm = ControlManager("cm_test")
q = zeros(initRobotData.nbJoints + 6)
dq = zeros(initRobotData.nbJoints)
bemfFactor = ones(initRobotData.nbJoints)
max_current = 30.0 * ones(initRobotData.nbJoints)
max_tau = 100.0 * ones(initRobotData.nbJoints)
percentageDriverDeadZoneCompensation = 20.0 * ones(initRobotData.nbJoints)
signWindowsFilterSize = ones(initRobotData.nbJoints)
tau = 100.0 * ones(initRobotData.nbJoints)
tau_predicted = 110.0 * ones(initRobotData.nbJoints)
pwmDes = 100.0 * ones(initRobotData.nbJoints)
currentDes = 100.0 * ones(initRobotData.nbJoints)
cm.controlDT = 0.005
# Initializing the input ports
# Setting the robot configuration
cm.i_max.value = max_current
cm.u_max.value = max_tau
cm.tau.value = tau
cm.tau_predicted.value = tau_predicted
cmInitRobotData = initRobotData()
cmInitRobotData.init_and_set_controller_manager(cm)
def create_ctrl_manager(conf, motor_params, dt, robot_name='robot'):
ctrl_manager = ControlManager("ctrl_man")
ctrl_manager.tau_predicted.value = NJ * (0.0, )
ctrl_manager.i_measured.value = NJ * (0.0, )
ctrl_manager.tau_max.value = NJ * (conf.TAU_MAX, )
ctrl_manager.i_max.value = NJ * (conf.CURRENT_MAX, )
ctrl_manager.u_max.value = NJ * (conf.CTRL_MAX, )
# Init should be called before addCtrlMode
# because the size of state vector must be known.
ctrl_manager.init(dt, conf.urdfFileName, robot_name)
# Set the map from joint name to joint ID
for key in conf.mapJointNameToID:
ctrl_manager.setNameToId(key, conf.mapJointNameToID[key])
# Set the map joint limits for each id
for key in conf.mapJointLimits:
ctrl_manager.setJointLimitsFromId(key,conf.mapJointLimits[key][0], \
conf.mapJointLimits[key][1])
# Set the force limits for each id
for key in conf.mapForceIdToForceLimits:
ctrl_manager.setForceLimitsFromId(key,tuple(conf.mapForceIdToForceLimits[key][0]), \
tuple(conf.mapForceIdToForceLimits[key][1]))
# Set the force sensor id for each sensor name
for key in conf.mapNameToForceId:
ctrl_manager.setForceNameToForceId(key, conf.mapNameToForceId[key])
# Set the map from the urdf joint list to the sot joint list
ctrl_manager.setJointsUrdfToSot(conf.urdftosot)
# Set the foot frame name
for key in conf.footFrameNames:
ctrl_manager.setFootFrameName(key, conf.footFrameNames[key])
# Set IMU hosting joint name
ctrl_manager.setImuJointName(conf.ImuJointName)
ctrl_manager.setRightFootForceSensorXYZ(conf.rightFootSensorXYZ)
ctrl_manager.setRightFootSoleXYZ(conf.rightFootSoleXYZ)
return ctrl_manager
def create_ctrl_manager(conf, motor_params, dt, robot_name='robot'):
ctrl_manager = ControlManager("ctrl_man");
# plug(ent.torque_ctrl.predictedJointsTorques, ctrl_manager.tau_predicted);
ctrl_manager.tau_predicted.value = NJ*(0.0,);
ctrl_manager.max_tau.value = NJ*(conf.CTRL_MANAGER_TAU_MAX,);
ctrl_manager.max_current.value = NJ*(conf.CTRL_MANAGER_CURRENT_MAX,);
ctrl_manager.percentageDriverDeadZoneCompensation.value = NJ*(conf.PERCENTAGE_DRIVER_DEAD_ZONE_COMPENSATION,);
ctrl_manager.percentage_bemf_compensation.value = conf.percentage_bemf_compensation;
ctrl_manager.current_sensor_offsets_low_level.value = conf.current_sensor_offsets_low_level;
ctrl_manager.iMaxDeadZoneCompensation.value = NJ*(conf.I_MAX_DEAD_ZONE_COMPENSATION,);
ctrl_manager.in_out_gain.value = NJ*(conf.IN_OUT_GAIN,);
ctrl_manager.kp_current.value = conf.kp_current;
ctrl_manager.ki_current.value = conf.ki_current;
ctrl_manager.bemfFactor.value = motor_params.K_bemf;
ctrl_manager.dead_zone_offsets.value = motor_params.deadzone;
ctrl_manager.cur_sens_gains.value = motor_params.cur_sens_gains;
ctrl_manager.currents.value = NJ*(0.0,);
# Init should be called before addCtrlMode
# because the size of state vector must be known.
ctrl_manager.init(dt, conf.urdfFileName, conf.CTRL_MANAGER_CURRENT_MAX, robot_name, conf.CURRENT_OFFSET_ITERS)
# Set the map from joint name to joint ID
for key in conf.mapJointNameToID:
ctrl_manager.setNameToId(key,conf.mapJointNameToID[key])
# Set the map joint limits for each id
for key in conf.mapJointLimits:
ctrl_manager.setJointLimitsFromId(key,conf.mapJointLimits[key][0], \
conf.mapJointLimits[key][1])
# Set the force limits for each id
for key in conf.mapForceIdToForceLimits:
ctrl_manager.setForceLimitsFromId(key,tuple(conf.mapForceIdToForceLimits[key][0]), \
tuple(conf.mapForceIdToForceLimits[key][1]))
# Set the force sensor id for each sensor name
for key in conf.mapNameToForceId:
ctrl_manager.setForceNameToForceId(key,conf.mapNameToForceId[key])
# Set the map from the urdf joint list to the sot joint list
ctrl_manager.setJointsUrdfToSot(conf.urdftosot)
# Set the foot frame name
for key in conf.footFrameNames:
ctrl_manager.setFootFrameName(key,conf.footFrameNames[key])
# Set IMU hosting joint name
ctrl_manager.setImuJointName(conf.ImuJointName)
ctrl_manager.setRightFootForceSensorXYZ(conf.rightFootSensorXYZ);
ctrl_manager.setRightFootSoleXYZ(conf.rightFootSoleXYZ);
ctrl_manager.setDefaultMaxCurrent(conf.CTRL_MANAGER_CURRENT_MAX)
return ctrl_manager;