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)