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 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.iterkeys(), u)) # dict: joint_id -> speed
return action
def think(self, perception):
if self.still:
return super(AngleInterpolationAgent, self).think(perception)
target_joints = self.angle_interpolation(perception)
self.target_joints.update(target_joints)
self.target_joints_log.append(self.target_joints.copy())
self.joints_log.append(perception.joint.copy())
result = super(AngleInterpolationAgent, self).think(perception)
self.actions_log.append(
dict(zip(JOINT_CMD_NAMES.iterkeys(), self.joint_controller.u)))
return result
def think(self, perception):
# What is SparkAgent.think method doing?
action = super(PIDAgent, self).think(perception)
# calculate control vector (speeds) from perception.joint, self.target_joints
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])
# Here, PIDController.control() method is called to determine this timestep's actions
u = self.joint_controller.control(target_angles, joint_angles)
# Prepare for return
action.speed = dict(zip(JOINT_CMD_NAMES.iterkeys(), 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
