def create_position_controller(device, estimator, dt=0.001, traj_gen=None): posCtrl = PositionController('pos_ctrl') posCtrl.Kp.value = tuple(kp_pos) posCtrl.Kd.value = tuple(kd_pos) posCtrl.Ki.value = tuple(ki_pos) posCtrl.dqRef.value = 30 * (0.0, ) plug(device.robotState, posCtrl.base6d_encoders) plug(estimator.jointsVelocities, posCtrl.jointsVelocities) plug(posCtrl.pwmDes, device.control) if (traj_gen != None): plug(traj_gen.q, posCtrl.qRef) posCtrl.init(dt) return posCtrl
def create_position_controller(robot, gains, dt=0.001, robot_name="robot"): posCtrl = PositionController('pos_ctrl') posCtrl.Kp.value = tuple(gains.kp_pos[round(dt, 3)]) posCtrl.Kd.value = tuple(gains.kd_pos[round(dt, 3)]) posCtrl.Ki.value = tuple(gains.ki_pos[round(dt, 3)]) posCtrl.dqRef.value = NJ * (0.0, ) plug(robot.device.robotState, posCtrl.base6d_encoders) try: # this works only in simulation plug(robot.device.jointsVelocities, posCtrl.jointsVelocities) except: plug(robot.filters.estimator_kin.dx, posCtrl.jointsVelocities) pass plug(posCtrl.pwmDes, robot.device.control) try: plug(robot.traj_gen.q, posCtrl.qRef) except: pass posCtrl.init(dt, robot_name) return posCtrl
from dynamic_graph.sot.torque_control.position_controller import PositionController from dynamic_graph.sot.torque_control.tests.robot_data_test import initRobotData from dynamic_graph.sot.torque_control.tests.test_control_manager import cm from numpy import ones, zeros # Instanciate the free flyer pc = PositionController("pc_test") q = zeros(initRobotData.nbJoints + 6) dq = zeros(initRobotData.nbJoints) qRef = zeros(initRobotData.nbJoints) dqRef = zeros(initRobotData.nbJoints) # Setting the robot configuration pc.base6d_encoders.value = q pc.qRef.value = qRef # Setting the robot velocities pc.jointsVelocities.value = dq pc.dqRef.value = dqRef Kp = ones(initRobotData.nbJoints) pc.Kp.value = Kp Ki = ones(initRobotData.nbJoints) pc.Ki.value = Ki Kd = ones(initRobotData.nbJoints) pc.Kd.value = Kd # Initializing the entity. pc.init(cm.controlDT, "control-manager-robot") # initRobotData.robotRef)