def init_agent(agent):
perception = Perception()
perception.time = start_time
for j in JOINT_CMD_NAMES.keys():
perception.joint[j] = 0
agent.perception = perception
def __init__(self, simspark_ip='localhost',
simspark_port=3100,
teamname='DAInamite',
player_id=0,
sync_mode=True):
super(PIDAgent, self).__init__(simspark_ip, simspark_port, teamname, player_id, sync_mode)
self.joint_names = JOINT_CMD_NAMES.keys()
number_of_joints = len(self.joint_names)
self.joint_controller = PIDController(dt=0.01, size=number_of_joints)
self.target_joints = {k: 0 for k in self.joint_names}
def __init__(self, simspark_ip='localhost',
simspark_port=3100,
teamname='DAInamite',
player_id=0,
sync_mode=True):
super(PIDAgent, self).__init__(simspark_ip, simspark_port, teamname, player_id, sync_mode)
self.joint_names = JOINT_CMD_NAMES.keys()
number_of_joints = len(self.joint_names)
self.joint_controller = PIDController(dt=0.01, size=number_of_joints)
self.target_joints = {k: 0 for k in self.joint_names}
def think(self, perception):
action = super(PIDAgent, self).think(perception)
'''calculate control vector (speeds) from
perception.joint: current joints' positions (dict: joint_id -> position (current))
self.target_joints: target positions (dict: joint_id -> position (target)) '''
joint_angles = np.asarray(
[perception.joint[joint_id] for joint_id in JOINT_CMD_NAMES])
target_angles = np.asarray([self.target_joints.get(joint_id,
perception.joint[joint_id]) for joint_id in JOINT_CMD_NAMES])
u = self.joint_controller.control(target_angles, joint_angles)
action.speed = dict(zip(JOINT_CMD_NAMES.keys(), u)) # dict: joint_id -> speed
return action
def __init__(self, simspark_ip='localhost',
simspark_port=3100,
teamname='DAInamite',
player_id=0,
sync_mode=True):
# Instantiating this agent calls SparkAgent's __init__ function
super(PIDAgent, self).__init__(simspark_ip, simspark_port,
teamname, player_id, sync_mode)
# Getting the names of the joints
self.joint_names = JOINT_CMD_NAMES.keys()
# Getting the number of joints (used right below)
number_of_joints = len(self.joint_names)
# Here I am instantiating a PIDController, defined above
self.joint_controller = PIDController(dt=0.01, size=number_of_joints)
# Setting all target joints to 0
self.target_joints = {k: 0 for k in self.joint_names}
def control(self, target, sensor):
"""apply PID control
@param target: reference values
@param sensor: current values from sensor
@return control signal
"""
e = target - sensor
speed = np.fromiter(dict(zip(JOINT_CMD_NAMES.keys(), self.u)).values(),
dtype=float)
predicted = self.u + speed * self.dt # angle(t) = angle(t-1) + speed * dt
self.y.append(predicted)
p = self.Kp + self.Ki * self.dt + self.Kd / self.dt
i = self.Kp + (2 * self.Kd) / self.dt
d = self.Kd / self.dt
self.u = self.y.popleft() + p * e - i * self.e1 + d * self.e2
self.e2 = self.e1
self.e1 = e
return self.u