def predict_state(robot, x_estimated, u, control_duration,
simulation_step_size, A, B, V, M, P_t, dynamic_problem):
"""
Predidcts the state at the next time step using an extended kalman filter
"""
if dynamic_problem:
current_state = v_double()
current_state[:] = x_estimated
control = v_double()
control[:] = u
control_error = v_double()
control_error[:] = [0.0 for i in xrange(len(u))]
result = v_double()
robot.propagate(current_state, control, control_error,
simulation_step_size, control_duration, result)
x_predicted = np.array([result[i] for i in xrange(len(result))])
x_tilde_dash, P_predicted = kalman_predict(x_estimated, u, A, B, P_t,
V, M)
return (x_predicted, P_predicted)
else:
x_predicted = np.dot(A, x_estimated) + np.dot(B, u)
x_tilde_dash, P_predicted = kalman_predict(x_estimated, u, A, B, P_t,
V, M)
return (x_predicted, P_predicted)
def is_in_collision(self, previous_state, state):
"""
Is the given end effector position in collision with an obstacle?
"""
collidable_obstacles = [o for o in self.obstacles if not o.isTraversable()]
joint_angles_goal = v_double()
joint_angles_goal[:] = [state[i] for i in xrange(self.robot_dof)]
collision_objects_goal = self.robot.createRobotCollisionObjects(joint_angles_goal)
if len(previous_state) > 0:
"""
Perform continuous collision checking if previous state is provided
"""
joint_angles_start = v_double()
joint_angles_start[:] = [previous_state[i] for i in xrange(self.robot_dof)]
collision_objects_start = self.robot.createRobotCollisionObjects(joint_angles_start)
for obstacle in collidable_obstacles:
for i in xrange(len(collision_objects_start)):
if obstacle.inCollisionContinuous([collision_objects_start[i], collision_objects_goal[i]]):
return True, obstacle
return False, None
else:
for obstacle in collidable_obstacles:
if obstacle.inCollisionDiscrete(collision_objects_goal):
return True, obstacle
return False, None
def setup(self,
robot,
M,
H,
W,
N,
C,
D,
dynamic_problem,
enforce_control_constraints):
self.M = M
self.H = H
self.W = W
self.N = N
self.C = C
self.D = D
self.dynamic_problem = dynamic_problem
self.control_constraints_enforced = enforce_control_constraints
active_joints = v_string()
robot.getActiveJoints(active_joints)
lower_position_constraints = v_double()
upper_position_constraints = v_double()
robot.getJointLowerPositionLimits(active_joints, lower_position_constraints)
robot.getJointUpperPositionLimits(active_joints, upper_position_constraints)
torque_limits = v_double()
robot.getJointTorqueLimits(active_joints, torque_limits)
torque_limits = [torque_limits[i] for i in xrange(len(torque_limits))]
torque_limits.extend([0.0 for i in xrange(len(active_joints))])
self.torque_limits = [torque_limits[i] for i in xrange(len(torque_limits))]
def is_in_collision(self, previous_state, state):
"""
Is the given end effector position in collision with an obstacle?
"""
collidable_obstacles = [
o for o in self.obstacles if not o.isTraversable()
]
joint_angles_goal = v_double()
joint_angles_goal[:] = [state[i] for i in xrange(self.robot_dof)]
collision_objects_goal = self.robot.createRobotCollisionObjects(
joint_angles_goal)
if len(previous_state) > 0:
"""
Perform continuous collision checking if previous state is provided
"""
joint_angles_start = v_double()
joint_angles_start[:] = [
previous_state[i] for i in xrange(self.robot_dof)
]
collision_objects_start = self.robot.createRobotCollisionObjects(
joint_angles_start)
for obstacle in collidable_obstacles:
for i in xrange(len(collision_objects_start)):
if obstacle.inCollisionContinuous([
collision_objects_start[i],
collision_objects_goal[i]
]):
return True, obstacle
return False, None
else:
for obstacle in collidable_obstacles:
if obstacle.inCollisionDiscrete(collision_objects_goal):
return True, obstacle
return False, None
def calc_covariance_value(self, robot, error, covariance_matrix, covariance_type='process'):
active_joints = v_string()
robot.getActiveJoints(active_joints)
if covariance_type == 'process':
if self.dynamic_problem:
torque_limits = v_double()
robot.getJointTorqueLimits(active_joints, torque_limits)
torque_limits = [torque_limits[i] for i in xrange(len(torque_limits))]
for i in xrange(len(torque_limits)):
torque_range = 2.0 * torque_limits[i]
covariance_matrix[i, i] = np.square((torque_range / 100.0) * error)
else:
for i in xrange(self.robot_dof):
covariance_matrix[i, i] = np.square((self.max_velocity / 100.0) * error)
else:
lower_position_limits = v_double()
upper_position_limits = v_double()
velocity_limits = v_double()
robot.getJointLowerPositionLimits(active_joints, lower_position_limits)
robot.getJointUpperPositionLimits(active_joints, upper_position_limits)
robot.getJointVelocityLimits(active_joints, velocity_limits)
for i in xrange(self.robot_dof):
position_range = upper_position_limits[i] - lower_position_limits[i]
covariance_matrix[i, i] = np.square((position_range / 100.0) * error)
velocity_range = 2.0 * velocity_limits[i]
covariance_matrix[i + self.robot_dof, i + self.robot_dof] = np.square((velocity_range / 100.0) * error)
return covariance_matrix
def visualize_x(self, robot, x, color=None):
cjvals = v_double()
cjvels = v_double()
cjvals[:] = [x[i] for i in xrange(len(x) / 2)]
cjvels[:] = [x[i] for i in xrange(len(x) / 2, len(x))]
particle_joint_values = v2_double()
robot.updateViewerValues(cjvals, cjvels, particle_joint_values,
particle_joint_values)
def get_random_state(robot):
active_joints = v_string()
robot.getActiveJoints(active_joints)
joint_lower_limits = v_double()
joint_upper_limits = v_double()
robot.getJointLowerPositionLimits(active_joints, joint_lower_limits)
robot.getJointUpperPositionLimits(active_joints, joint_upper_limits)
state = [np.random.uniform(joint_lower_limits[i], joint_upper_limits[i]) for i in xrange(len(active_joints))]
return np.array(state)
def is_terminal(self, state):
ja = v_double()
ja[:] = [state[j] for j in xrange(len(state) / 2)]
ee_position_arr = v_double()
self.robot.getEndEffectorPosition(ja, ee_position_arr)
ee_position = np.array([ee_position_arr[j] for j in xrange(len(ee_position_arr))])
norm = np.linalg.norm(ee_position - self.goal_position)
if norm - 0.01 <= self.goal_radius:
return True
return False
def is_terminal(self, state):
ja = v_double()
ja[:] = [state[j] for j in xrange(len(state) / 2)]
ee_position_arr = v_double()
self.robot.getEndEffectorPosition(ja, ee_position_arr)
ee_position = np.array([ee_position_arr[j] for j in xrange(len(ee_position_arr))])
norm = np.linalg.norm(ee_position - self.goal_position)
if norm - 0.01 <= self.goal_radius:
return True
return False
def visualize_x(self, robot, x, color=None):
cjvals = v_double()
cjvels = v_double()
cjvals[:] = [x[i] for i in xrange(len(x) / 2)]
cjvels[:] = [x[i] for i in xrange(len(x) / 2, len(x))]
particle_joint_values = v2_double()
robot.updateViewerValues(cjvals,
cjvels,
particle_joint_values,
particle_joint_values)
def get_linear_model_matrices(self, state_path, control_path,
control_durations):
As = []
Bs = []
Vs = []
Ms = []
Hs = []
Ws = []
Ns = []
if self.dynamic_problem:
for i in xrange(len(state_path)):
state = v_double()
control = v_double()
state[:] = state_path[i]
control[:] = control_path[i]
A = self.robot.getProcessMatrices(state, control,
control_durations[i])
Matr_list = [A[j] for j in xrange(len(A))]
A_list = np.array(
[Matr_list[j] for j in xrange(len(state)**2)])
start_index = len(state)**2
B_list = np.array([
Matr_list[j] for j in xrange(
start_index, start_index + (len(state) *
(len(state) / 2)))
])
start_index = start_index + (len(state) * (len(state) / 2))
V_list = np.array([
Matr_list[j] for j in xrange(
start_index, start_index + (len(state) *
(len(state) / 2)))
])
A_Matr = A_list.reshape(len(state), len(state)).T
B_Matr = B_list.reshape(len(state) / 2, len(state)).T
V_Matr = V_list.reshape(len(state) / 2, len(state)).T
As.append(A_Matr)
Bs.append(B_Matr)
Vs.append(V_Matr)
Ms.append(self.M)
Hs.append(self.H)
Ws.append(self.W)
Ns.append(self.N)
else:
for i in xrange(len(state_path) + 1):
As.append(self.A)
Bs.append(self.B)
Vs.append(self.V)
Ms.append(self.M)
Hs.append(self.H)
Ws.append(self.W)
Ns.append(self.N)
return As, Bs, Vs, Ms, Hs, Ws, Ns
def get_linear_model_matrices(robot,
state_path,
control_path,
control_durations,
dynamic_problem,
M,
H,
W,
N):
""" Get the linearized model matrices along a given nominal path
"""
As = []
Bs = []
Vs = []
Ms = []
Hs = []
Ws = []
Ns = []
if dynamic_problem:
for i in xrange(len(state_path)):
state = v_double()
control = v_double()
state[:] = state_path[i]
control[:] = control_path[i]
A = robot.getProcessMatrices(state, control, control_durations[i])
Matr_list = [A[j] for j in xrange(len(A))]
A_list = np.array([Matr_list[j] for j in xrange(len(state)**2)])
start_index = len(state)**2
B_list = np.array([Matr_list[j] for j in xrange(start_index,
start_index + (len(state) * (len(state) / 2)))])
start_index = start_index + (len(state) * (len(state) / 2))
V_list = np.array([Matr_list[j] for j in xrange(start_index,
start_index + (len(state) * (len(state) / 2)))])
A_Matr = A_list.reshape(len(state), len(state)).T
B_Matr = B_list.reshape(len(state)/ 2, len(state)).T
V_Matr = V_list.reshape(len(state) / 2, len(state)).T
As.append(A_Matr)
Bs.append(B_Matr)
Vs.append(V_Matr)
Ms.append(M)
Hs.append(H)
Ws.append(W)
Ns.append(N)
else:
for i in xrange(len(state_path) + 1):
As.append(self.A)
Bs.append(self.B)
Vs.append(self.V)
Ms.append(self.M)
Hs.append(self.H)
Ws.append(self.W)
Ns.append(self.N)
return As, Bs, Vs, Ms, Hs, Ws, Ns
def check_constraints(robot, state):
active_joints = v_string()
robot.getActiveJoints(active_joints)
joint_lower_limits = v_double()
joint_upper_limits = v_double()
robot.getJointLowerPositionLimits(active_joints, joint_lower_limits)
robot.getJointUpperPositionLimits(active_joints, joint_upper_limits)
for i in xrange(len(state)):
if (state[i] < joint_lower_limits[i] + 0.00000001
or state[i] > joint_upper_limits[i] - 0.00000001):
return False
return True
def check_constraints(robot, state):
active_joints = v_string()
robot.getActiveJoints(active_joints)
joint_lower_limits = v_double()
joint_upper_limits = v_double()
robot.getJointLowerPositionLimits(active_joints, joint_lower_limits)
robot.getJointUpperPositionLimits(active_joints, joint_upper_limits)
for i in xrange(len(state)):
if (state[i] < joint_lower_limits[i] + 0.00000001 or
state[i] > joint_upper_limits[i] - 0.00000001):
return False
return True
def is_terminal(self, x):
"""
Check if x is terminal
"""
state = v_double()
state[:] = [x[j] for j in xrange(len(x) / 2)]
ee_position_arr = v_double()
self.robot.getEndEffectorPosition(state, ee_position_arr)
ee_position = np.array([ee_position_arr[j] for j in xrange(len(ee_position_arr))])
norm = np.linalg.norm(ee_position - self.goal_position)
if norm - 0.01 <= self.goal_radius:
return True
return False
def setup(self, A, B, C, D, H, M, N, V, W, robot, sample_size, obstacles,
goal_position, goal_radius, show_viewer, model_file, env_file,
num_cores):
self.robot = robot
self.A = A
self.B = B
self.C = C
self.D = D
self.H = H
self.M = M
self.N = N
self.V = V
self.W = W
self.robot_dof = robot.getDOF()
self.obstacles = obstacles
active_joints = v_string()
robot.getActiveJoints(active_joints)
lower_position_constraints = v_double()
upper_position_constraints = v_double()
velocity_constraints = v_double()
robot.getJointLowerPositionLimits(active_joints,
lower_position_constraints)
robot.getJointUpperPositionLimits(active_joints,
upper_position_constraints)
robot.getJointVelocityLimits(active_joints, velocity_constraints)
self.lower_position_constraints = [
lower_position_constraints[i]
for i in xrange(len(lower_position_constraints))
]
self.upper_position_constraints = [
upper_position_constraints[i]
for i in xrange(len(upper_position_constraints))
]
self.velocity_constraints = [
velocity_constraints[i] for i in xrange(len(velocity_constraints))
]
self.constraints_enforced = robot.constraintsEnforced()
self.sample_size = sample_size
self.num_cores = num_cores
#self.num_cores = cpu_count() - 1
#self.num_cores = 2
self.goal_position = goal_position
self.goal_radius = goal_radius
self.mutex = Lock()
self.dynamic_problem = False
self.show_viewer = show_viewer
self.model_file = model_file
self.env_file = env_file
def is_terminal(self, x):
"""
Check if x is terminal
"""
state = v_double()
state[:] = [x[j] for j in xrange(len(x) / 2)]
ee_position_arr = v_double()
self.robot.getEndEffectorPosition(state, ee_position_arr)
ee_position = np.array(
[ee_position_arr[j] for j in xrange(len(ee_position_arr))])
norm = np.linalg.norm(ee_position - self.goal_position)
if norm - 0.01 <= self.goal_radius:
return True
return False
def setup(self, A, B, C, D, H, M, N, V, W,
robot,
sample_size,
obstacles,
goal_position,
goal_radius,
show_viewer,
model_file,
env_file,
num_cores):
self.robot = robot
self.A = A
self.B = B
self.C = C
self.D = D
self.H = H
self.M = M
self.N = N
self.V = V
self.W = W
self.robot_dof = robot.getDOF()
self.obstacles = obstacles
active_joints = v_string()
robot.getActiveJoints(active_joints)
lower_position_constraints = v_double()
upper_position_constraints = v_double()
velocity_constraints = v_double()
robot.getJointLowerPositionLimits(active_joints, lower_position_constraints)
robot.getJointUpperPositionLimits(active_joints, upper_position_constraints)
robot.getJointVelocityLimits(active_joints, velocity_constraints)
self.lower_position_constraints = [lower_position_constraints[i] for i in xrange(len(lower_position_constraints))]
self.upper_position_constraints = [upper_position_constraints[i] for i in xrange(len(upper_position_constraints))]
self.velocity_constraints = [velocity_constraints[i] for i in xrange(len(velocity_constraints))]
self.constraints_enforced = robot.constraintsEnforced()
self.sample_size = sample_size
self.num_cores = num_cores
#self.num_cores = cpu_count() - 1
#self.num_cores = 2
self.goal_position = goal_position
self.goal_radius = goal_radius
self.mutex = Lock()
self.dynamic_problem = False
self.show_viewer = show_viewer
self.model_file = model_file
self.env_file = env_file
def is_terminal(self, state):
ee_position_arr = v_double()
self.robot.getEndEffectorPosition(state, ee_position_arr)
ee_position = np.array([ee_position_arr[j] for j in xrange(len(ee_position_arr))])
if np.linalg.norm(ee_position - self.goal_position) < self.goal_radius:
return True
return False
def get_random_state(robot):
active_joints = v_string()
robot.getActiveJoints(active_joints)
joint_lower_limits = v_double()
joint_upper_limits = v_double()
robot.getJointLowerPositionLimits(active_joints, joint_lower_limits)
robot.getJointUpperPositionLimits(active_joints, joint_upper_limits)
state = [np.random.uniform(joint_lower_limits[i], joint_upper_limits[i]) for i in xrange(len(active_joints))]
if len(active_joints) == 6:
state = [1.91913489548,
0.41647636025,
-0.641053542305,
-0.628739802863,
-0.0964141860436,
0.336857099326]
return np.array(state)
def generate(self,
start_state,
goal_position,
goal_threshold,
num_generated_goal_states):
"""
Goal position is w.r.t. base frame
"""
goal_position = self.transform_goal(goal_position)
possible_ik_solutions = ik.get_goal_states(self.robot,
goal_position,
self.obstacles,
num=num_generated_goal_states)
solutions = []
n = 0
logging.warn("IKSolutionGenerator: " + str(len(possible_ik_solutions)) + " possible ik solutions found")
for i in xrange(len(possible_ik_solutions)):
logging.info("IKSolutionGenerator: Checking ik solution " + str(i) + " for validity")
ik_solution = [possible_ik_solutions[i][k] for k in xrange(len(start_state) / 2)]
ik_solution.extend([0.0 for j in xrange(len(start_state) / 2)])
goal_states = v2_double()
goal_state = v_double()
goal_state[:] = ik_solution
goal_states[:] = [goal_state]
goal_position_vec = v_double()
goal_position_vec[:] = goal_position
print "check for " + str([[goal_states[i][j] for j in xrange(len(goal_states[i]))] for i in xrange(len(goal_states))])
self.path_planner.setGoalStates(goal_states, goal_position_vec, goal_threshold)
#self.path_planner.set_start_and_goal(start_state, [ik_solution], goal_position, goal_threshold)
start_state_vec = v_double()
start_state_vec[:] = start_state
path = self.path_planner.solve(start_state_vec, self.path_timeout)
if len(path) != 0:
logging.warn("IKSolutionGenerator: ik solution " + str(i) + " is a valid ik solution")
solutions.append(ik_solution)
else:
logging.warn("IKSolutionGenerator: Path has length 0")
n += 1
self.path_planner = None
if not len(solutions) == 0:
print "IKSolutionGenerator: Found " + str(len(solutions)) + " valid goal states"
return solutions
else:
logging.error("IKSoultionGenerator: Couldn't find a valid IK solution. Defined problem seems to be infeasible.")
self.path_planner = None
return []
def show_nominal_path(self, robot, path):
if self.show_viewer:
robot.removePermanentViewerParticles()
particle_joint_values = v2_double()
particle_joint_colors = v2_double()
pjvs = []
for p in path:
particle = v_double()
particle[:] = [p[i] for i in xrange(len(p) / 2)]
pjvs.append(particle)
color = v_double()
color[:] = [0.0, 0.0, 0.0, 0.7]
particle_joint_colors.append(color)
particle_joint_values[:] = pjvs
robot.addPermanentViewerParticles(particle_joint_values,
particle_joint_colors)
print "Permanent particles added"
def get_random_state(robot):
active_joints = v_string()
robot.getActiveJoints(active_joints)
joint_lower_limits = v_double()
joint_upper_limits = v_double()
robot.getJointLowerPositionLimits(active_joints, joint_lower_limits)
robot.getJointUpperPositionLimits(active_joints, joint_upper_limits)
state = [
np.random.uniform(joint_lower_limits[i], joint_upper_limits[i])
for i in xrange(len(active_joints))
]
if len(active_joints) == 6:
state = [
1.91913489548, 0.41647636025, -0.641053542305, -0.628739802863,
-0.0964141860436, 0.336857099326
]
return np.array(state)
def is_terminal(self, state):
ee_position_arr = v_double()
self.robot.getEndEffectorPosition(state, ee_position_arr)
ee_position = np.array(
[ee_position_arr[j] for j in xrange(len(ee_position_arr))])
if np.linalg.norm(ee_position - self.goal_position) < self.goal_radius:
return True
return False
def show_nominal_path(self, robot, path):
if self.show_viewer:
robot.removePermanentViewerParticles()
particle_joint_values = v2_double()
particle_joint_colors = v2_double()
pjvs = []
for p in path:
particle = v_double()
particle[:] = [p[i] for i in xrange(len(p) / 2)]
pjvs.append(particle)
color = v_double()
color[:] = [0.0, 0.0, 0.0, 0.7]
particle_joint_colors.append(color)
particle_joint_values[:] = pjvs
robot.addPermanentViewerParticles(particle_joint_values,
particle_joint_colors)
print "Permanent particles added"
def get_linearized_next_state_prop(self, x, x_nominal, u, u_nominal,
control_error, control_duration):
current_state = v_double()
current_state[:] = x
nominal_state = v_double()
nominal_state[:] = x_nominal
control = v_double()
control[:] = u
nominal_control = v_double()
nominal_control[:] = u_nominal
control_e = v_double()
control_e[:] = control_error
result = v_double()
result2 = v_double()
self.robot.propagate_first_order(current_state, control, control_e,
nominal_state, nominal_control, 0.001,
control_duration, result)
self.robot.propagate_second_order(current_state, control, control_e,
nominal_state, nominal_control,
0.001, control_duration, result2)
r = np.array([result[i] for i in xrange(len(result))])
r2 = np.array([result2[i] for i in xrange(len(result2))])
print "dev first order " + str(r)
print "dev second order " + str(r2)
return (r, r2)
def show_nominal_path(self, path):
""" Shows the nominal path in the viewer """
if self.show_viewer:
self.robot.removePermanentViewerParticles()
particle_joint_values = v2_double()
particle_joint_colors = v2_double()
pjvs = []
for p in path:
particle = v_double()
particle[:] = [p[i] for i in xrange(len(p) / 2)]
pjvs.append(particle)
color = v_double()
color[:] = [0.0, 0.0, 0.0, 0.7]
particle_joint_colors.append(color)
particle_joint_values[:] = pjvs
self.robot.addPermanentViewerParticles(particle_joint_values,
particle_joint_colors)
def in_collision(robot, state, obstacles):
collidable_obstacles = [o for o in obstacles if not o.isTraversable()]
joint_angles = v_double()
joint_angles[:] = [state[i] for i in xrange(len(state))]
collision_objects = robot.createRobotCollisionObjects(joint_angles)
for obstacle in collidable_obstacles:
if obstacle.inCollisionDiscrete(collision_objects):
return True
return False
def in_collision(robot, state, obstacles):
collidable_obstacles = [o for o in obstacles if not o.isTraversable()]
joint_angles = v_double()
joint_angles[:] = [state[i] for i in xrange(len(state))]
collision_objects = robot.createRobotCollisionObjects(joint_angles)
for obstacle in collidable_obstacles:
if obstacle.inCollisionDiscrete(collision_objects):
return True
return False
def show_nominal_path(self, path):
""" Shows the nominal path in the viewer """
if self.show_viewer:
self.robot.removePermanentViewerParticles()
particle_joint_values = v2_double()
particle_joint_colors = v2_double()
pjvs = []
for p in path:
particle = v_double()
particle[:] = [p[i] for i in xrange(len(p) / 2)]
pjvs.append(particle)
color = v_double()
color[:] = [0.0, 0.0, 0.0, 0.7]
particle_joint_colors.append(color)
particle_joint_values[:] = pjvs
self.robot.addPermanentViewerParticles(particle_joint_values,
particle_joint_colors)
def show_state_and_cov(self, state, cov, samples):
joint_values = v_double()
joint_values[:] = [state[i] for i in xrange(len(state) / 2)]
joint_velocities = v_double()
joint_velocities[:] = [state[i] for i in xrange(len(state) / 2, len(state))]
particles = v2_double()
particle_joint_colors = v2_double()
#samples = self.sample_multivariate_normal(state, cov, 50)
for s in samples:
particle = v_double()
particle_color = v_double()
particle[:] = [s[i] for i in xrange(len(s) / 2)]
particle_color[:] = [0.5, 0.5, 0.5, 0.5]
particles.append(particle)
particle_joint_colors.append(particle_color)
self.robot.updateViewerValues(joint_values,
joint_velocities,
particles,
particle_joint_colors)
def show_state_and_cov(self, state, cov, samples):
joint_values = v_double()
joint_values[:] = [state[i] for i in xrange(len(state) / 2)]
joint_velocities = v_double()
joint_velocities[:] = [
state[i] for i in xrange(len(state) / 2, len(state))
]
particles = v2_double()
particle_joint_colors = v2_double()
#samples = self.sample_multivariate_normal(state, cov, 50)
for s in samples:
particle = v_double()
particle_color = v_double()
particle[:] = [s[i] for i in xrange(len(s) / 2)]
particle_color[:] = [0.5, 0.5, 0.5, 0.5]
particles.append(particle)
particle_joint_colors.append(particle_color)
self.robot.updateViewerValues(joint_values, joint_velocities,
particles, particle_joint_colors)
def get_jacobian(robot, state):
s = v_double()
s[:] = [state[i] for i in xrange(len(state))]
ee_jacobian = v2_double()
robot.getEndEffectorJacobian(s, ee_jacobian)
jac = []
for i in xrange(len(ee_jacobian)):
jac_el = [ee_jacobian[i][j] for j in xrange(len(ee_jacobian[i]))]
jac.append(jac_el)
return np.array(jac)
def init_environment(self):
self.obstacles = self.utils.loadObstaclesXML(self.environment_file)
goal_area = v_double()
self.utils.loadGoalArea(self.environment_file, goal_area)
if len(goal_area) == 0:
print "ERROR: Your environment file doesn't define a goal area"
return False
self.goal_position = [goal_area[i] for i in xrange(0, 3)]
self.goal_radius = goal_area[3]
return True
def init_environment(self):
self.obstacles = self.utils.loadObstaclesXML(self.environment_file)
goal_area = v_double()
self.utils.loadGoalArea(self.environment_file, goal_area)
if len(goal_area) == 0:
print "ERROR: Your environment file doesn't define a goal area"
return False
self.goal_position = [goal_area[i] for i in xrange(0, 3)]
self.goal_radius = goal_area[3]
return True
def get_expected_state_reward(self, mean, cov):
with self.mutex:
np.random.seed()
samples = self.sample_valid_states(mean, cov, self.sample_size)
pdf = multivariate_normal.pdf(samples, mean, cov, allow_singular=True)
if np.isscalar(pdf):
pdf = [pdf]
else:
pdf /= sum(pdf)
expected_reward = 0.0
expected_reward2 = 0.0
num_collisions = 0
terminal = False
for i in xrange(len(samples)):
vec = [samples[i][j] for j in xrange(self.robot_dof)]
joint_angles = v_double()
joint_angles[:] = vec
collides = False
terminal = False
collision_objects = self.robot.createRobotCollisionObjects(joint_angles)
for obstacle in self.obstacles:
if obstacle.inCollisionDiscrete(collision_objects) and not obstacle.isTraversable():
expected_reward -= self.collision_penalty
#expected_reward2 -= pdf[i] * self.collision_penalty
collides = True
num_collisions += 1
break
if not collides:
if self.is_terminal(joint_angles):
expected_reward += self.exit_reward
#expected_reward2 += pdf[i] * self.exit_reward
terminal = True
else:
expected_reward -= self.step_penalty
#expected_reward2 -= pdf[i] * self.step_penalty
#print "pdf[i] " + str(pdf[i])
#expected_reward -= pdf[i] * self.step_penalty
expected_reward /= len(samples)
'''print "========"
print "num collisions " + str(num_collisions)
print "expected num collisions " + str(float(num_collisions / float(self.sample_size)))
print "expected reward " + str(expected_reward)
print "========"'''
#time.sleep(3)
"""
Note that the X = collision is a binary random variable, therefore E[X] = p, p = P(X=1)
where X = 1 denotes collision
"""
expected_num_collisions = float(num_collisions / self.sample_size)
return (expected_reward,
terminal,
float(num_collisions / float(self.sample_size)),
samples)
def show_nominal_path(self, path):
""" Shows the nominal path in the viewer """
if not self.viewer_initialized:
self.robot.setupViewer(self.robot_file, self.environment_file)
self.viewer_initialized = True
self.robot.removePermanentViewerParticles()
particle_joint_values = v2_double()
particle_joint_colors = v2_double()
pjvs = []
for p in path:
particle = v_double()
particle[:] = [p[i] for i in xrange(len(p) / 2)]
pjvs.append(particle)
color = v_double()
color[:] = [0.0, 0.0, 0.0, 0.7]
particle_joint_colors.append(color)
particle_joint_values[:] = pjvs
self.robot.addPermanentViewerParticles(particle_joint_values,
particle_joint_colors)
def get_expected_state_reward(self, mean, cov):
with self.mutex:
np.random.seed()
samples = self.sample_valid_states(mean, cov, self.sample_size)
pdf = multivariate_normal.pdf(samples, mean, cov, allow_singular=True)
if np.isscalar(pdf):
pdf = [pdf]
else:
pdf /= sum(pdf)
expected_reward = 0.0
expected_reward2 = 0.0
num_collisions = 0
terminal = False
for i in xrange(len(samples)):
vec = [samples[i][j] for j in xrange(self.robot_dof)]
joint_angles = v_double()
joint_angles[:] = vec
collides = False
terminal = False
collision_objects = self.robot.createRobotCollisionObjects(
joint_angles)
for obstacle in self.obstacles:
if obstacle.inCollisionDiscrete(
collision_objects) and not obstacle.isTraversable():
expected_reward -= self.collision_penalty
#expected_reward2 -= pdf[i] * self.collision_penalty
collides = True
num_collisions += 1
break
if not collides:
if self.is_terminal(joint_angles):
expected_reward += self.exit_reward
#expected_reward2 += pdf[i] * self.exit_reward
terminal = True
else:
expected_reward -= self.step_penalty
#expected_reward2 -= pdf[i] * self.step_penalty
#print "pdf[i] " + str(pdf[i])
#expected_reward -= pdf[i] * self.step_penalty
expected_reward /= len(samples)
'''print "========"
print "num collisions " + str(num_collisions)
print "expected num collisions " + str(float(num_collisions / float(self.sample_size)))
print "expected reward " + str(expected_reward)
print "========"'''
#time.sleep(3)
"""
Note that the X = collision is a binary random variable, therefore E[X] = p, p = P(X=1)
where X = 1 denotes collision
"""
expected_num_collisions = float(num_collisions / self.sample_size)
return (expected_reward, terminal,
float(num_collisions / float(self.sample_size)), samples)
def show_nominal_path(self, path):
""" Shows the nominal path in the viewer """
if not self.viewer_initialized:
self.robot.setupViewer(self.robot_file, self.environment_file)
self.viewer_initialized = True
self.robot.removePermanentViewerParticles()
particle_joint_values = v2_double()
particle_joint_colors = v2_double()
pjvs = []
for p in path:
particle = v_double()
particle[:] = [p[i] for i in xrange(len(p) / 2)]
pjvs.append(particle)
color = v_double()
color[:] = [0.0, 0.0, 0.0, 0.7]
particle_joint_colors.append(color)
particle_joint_values[:] = pjvs
self.robot.addPermanentViewerParticles(particle_joint_values,
particle_joint_colors)
def calc_covariance_value(self,
robot,
error,
covariance_matrix,
covariance_type='process'):
active_joints = v_string()
robot.getActiveJoints(active_joints)
if covariance_type == 'process':
if self.dynamic_problem:
torque_limits = v_double()
robot.getJointTorqueLimits(active_joints, torque_limits)
torque_limits = [
torque_limits[i] for i in xrange(len(torque_limits))
]
for i in xrange(len(torque_limits)):
torque_range = 2.0 * torque_limits[i]
covariance_matrix[i, i] = np.square(
(torque_range / 100.0) * error)
else:
for i in xrange(self.robot_dof):
covariance_matrix[i, i] = np.square(
(self.max_velocity / 100.0) * error)
else:
lower_position_limits = v_double()
upper_position_limits = v_double()
velocity_limits = v_double()
robot.getJointLowerPositionLimits(active_joints,
lower_position_limits)
robot.getJointUpperPositionLimits(active_joints,
upper_position_limits)
robot.getJointVelocityLimits(active_joints, velocity_limits)
for i in xrange(self.robot_dof):
position_range = upper_position_limits[
i] - lower_position_limits[i]
covariance_matrix[i, i] = np.square(
(position_range / 100.0) * error)
velocity_range = 2.0 * velocity_limits[i]
covariance_matrix[i + self.robot_dof,
i + self.robot_dof] = np.square(
(velocity_range / 100.0) * error)
return covariance_matrix
def get_jacobian(robot, state):
s = v_double()
s[:] = [state[i] for i in xrange(len(state))]
ee_jacobian = v2_double()
robot.getEndEffectorJacobian(s, ee_jacobian)
jac = []
for i in xrange(len(ee_jacobian)):
jac_el = [ee_jacobian[i][j] for j in xrange(len(ee_jacobian[i]))]
jac.append(jac_el)
return np.array(jac)
def apply_control(self,
x,
u,
control_duration,
A,
B,
V,
M):
""" Applies a control input 'u' to the system which
is in state 'x' for the duration of 'control_duration'.
A, B, and V are used for the linear problem. M is used
to sample a control (or state) error
"""
x_new = None
ce = None
if self.dynamic_problem:
current_state = v_double()
current_state[:] = x
control = v_double()
control[:] = u
control_error = v_double()
ce = self.sample_control_error(M)
control_error[:] = ce
result = v_double()
self.robot.propagate(current_state,
control,
control_error,
self.simulation_step_size,
control_duration,
result)
x_new = np.array([result[i] for i in xrange(len(result))])
else:
ce = self.get_random_joint_angles([0.0 for i in xrange(M.shape[0])], M)
x_new = np.dot(A, x) + np.dot(B, u) + np.dot(V, ce)
#x_new = self.normalize_state(x_new)
if self.enforce_constraints:
x_new = self.check_constraints(x_new)
return x_new, ce
def visualize_paths(self, robot, paths, colors=None):
robot.removePermanentViewerParticles()
particle_joint_values = v2_double()
particle_joint_colors = v2_double()
pjvs = []
for k in xrange(len(paths)):
for p in paths[k]:
particle = v_double()
particle[:] = [p[i] for i in xrange(len(p) / 2)]
pjvs.append(particle)
if colors == None:
color = v_double()
color[:] = [0.0, 0.0, 0.0, 0.7]
particle_joint_colors.append(color)
else:
c = v_double()
c[:] = color
particle_joint_colors.append(c)
particle_joint_values[:] = pjvs
self.robot.addPermanentViewerParticles(particle_joint_values,
particle_joint_colors)
def visualize_paths(self, robot, paths, colors=None):
robot.removePermanentViewerParticles()
particle_joint_values = v2_double()
particle_joint_colors = v2_double()
pjvs = []
for k in xrange(len(paths)):
for p in paths[k]:
particle = v_double()
particle[:] = [p[i] for i in xrange(len(p) / 2)]
pjvs.append(particle)
if colors == None:
color = v_double()
color[:] = [0.0, 0.0, 0.0, 0.7]
particle_joint_colors.append(color)
else:
c = v_double()
c[:] = color
particle_joint_colors.append(c)
particle_joint_values[:] = pjvs
self.robot.addPermanentViewerParticles(particle_joint_values,
particle_joint_colors)
def apply_control(self,
x,
u,
control_duration,
A,
B,
V,
M):
""" Applies a control input 'u' to the system which
is in state 'x' for the duration of 'control_duration'.
A, B, and V are used for the linear problem. M is used
to sample a control (or state) error
"""
x_new = None
ce = None
if self.dynamic_problem:
current_state = v_double()
current_state[:] = x
control = v_double()
control[:] = u
control_error = v_double()
ce = self.sample_control_error(M)
control_error[:] = ce
result = v_double()
self.robot.propagate(current_state,
control,
control_error,
self.simulation_step_size,
control_duration,
result)
x_new = np.array([result[i] for i in xrange(len(result))])
else:
ce = self.get_random_joint_angles([0.0 for i in xrange(M.shape[0])], M)
x_new = np.dot(A, x) + np.dot(B, u) + np.dot(V, ce)
#x_new = self.normalize_state(x_new)
if self.enforce_constraints:
x_new = self.check_constraints(x_new)
return x_new, ce
def setup_scene(self, environment_file, robot):
""" Load the obstacles """
self.obstacles = self.utils.loadObstaclesXML(self.abs_path + "/" +
environment_file)
""" Load the goal area """
goal_area = v_double()
self.utils.loadGoalArea(self.abs_path + "/" + environment_file,
goal_area)
if len(goal_area) == 0:
print "ERROR: Your environment file doesn't define a goal area"
return False
self.goal_position = [goal_area[i] for i in xrange(0, 3)]
self.goal_radius = goal_area[3]
return True
def predict_state(robot,
x_estimated,
u,
control_duration,
simulation_step_size,
A,
B,
V,
M,
P_t,
dynamic_problem):
"""
Predidcts the state at the next time step using an extended kalman filter
"""
if dynamic_problem:
current_state = v_double()
current_state[:] = x_estimated
control = v_double()
control[:] = u
control_error = v_double()
control_error[:] = [0.0 for i in xrange(len(u))]
result = v_double()
robot.propagate(current_state,
control,
control_error,
simulation_step_size,
control_duration,
result)
x_predicted = np.array([result[i] for i in xrange(len(result))])
x_tilde_dash, P_predicted = kalman_predict(x_estimated, u, A, B, P_t, V, M)
return (x_predicted, P_predicted)
else:
x_predicted = np.dot(A, x_estimated) + np.dot(B, u)
x_tilde_dash, P_predicted = kalman_predict(x_estimated, u, A, B, P_t, V, M)
return (x_predicted, P_predicted)
def update_viewer(self,
x,
x_estimate,
z,
control_duration=None,
colliding_obstacle=None):
""" Update the viewer to display the state 'x' of the robot
Optionally sleep for 'control_duration'
"""
if self.show_viewer:
cjvals = v_double()
cjvels = v_double()
cjvals_arr = [x[i] for i in xrange(len(x) / 2)]
cjvels_arr = [x[i] for i in xrange(len(x) / 2, len(x))]
cjvals[:] = cjvals_arr
cjvels[:] = cjvels_arr
particle_joint_values = v2_double()
pjv = v_double()
pjv[:] = [z[i] for i in xrange(len(z))]
pjv_estimate = v_double()
pjv_estimate[:] = [x_estimate[i] for i in xrange(len(x_estimate))]
particle_joint_values.append(pjv)
particle_joint_values.append(pjv_estimate)
particle_joint_colors = v2_double()
pja = v_double()
pja[:] = [0.5, 0.5, 0.9, 0.0]
pja_estimate = v_double()
pja_estimate[:] = [0.2, 0.8, 0.2, 0.0]
particle_joint_colors.append(pja)
particle_joint_colors.append(pja_estimate)
self.robot.updateViewerValues(cjvals, cjvels,
particle_joint_values,
particle_joint_colors)
for o in self.obstacles:
self.robot.setObstacleColor(o.getName(),
o.getStandardDiffuseColor(),
o.getStandardAmbientColor())
if colliding_obstacle != None:
diffuse_col = v_double()
ambient_col = v_double()
diffuse_col[:] = [0.5, 0.0, 0.0, 0.0]
ambient_col[:] = [0.8, 0.0, 0.0, 0.0]
self.robot.setObstacleColor(colliding_obstacle.getName(),
diffuse_col, ambient_col)
time.sleep(control_duration)
time.sleep(1.0)
def setup_scene(self,
environment_file,
robot):
""" Load the obstacles """
self.obstacles = self.utils.loadObstaclesXML(self.abs_path + "/" + environment_file)
""" Load the goal area """
goal_area = v_double()
self.utils.loadGoalArea(self.abs_path + "/" + environment_file, goal_area)
if len(goal_area) == 0:
print "ERROR: Your environment file doesn't define a goal area"
return False
self.goal_position = [goal_area[i] for i in xrange(0, 3)]
self.goal_radius = goal_area[3]
return True
def play_states(self, states, col, particles):
if not self.viewer_initialized:
self.robot.setupViewer(self.robot_file, self.environment_file)
self.viewer_initialized = True
for i in xrange(len(states)):
cjvals = v_double()
cjvels = v_double()
cjvals_arr = [states[i][j] for j in xrange(len(states[i]) / 2)]
cjvels_arr = [states[i][j] for j in xrange(len(states[i]) / 2, len(states[i]))]
cjvals[:] = cjvals_arr
cjvels[:] = cjvels_arr
particle_joint_values = v2_double()
particle_joint_colors = v2_double()
if i > 1 and len(particles) > 0:
for p in particles[i-1]:
particle = v_double()
particle_color = v_double()
particle[:] = [p[k] for k in xrange(len(p) / 2)]
particle_color[:] = [0.5, 0.5, 0.5, 0.5]
particle_joint_values.append(particle)
particle_joint_colors.append(particle_color)
self.robot.updateViewerValues(cjvals,
cjvels,
particle_joint_values,
particle_joint_colors)
for o in self.obstacles:
self.robot.setObstacleColor(o.getName(),
o.getStandardDiffuseColor(),
o.getStandardAmbientColor())
try:
if col[i] == True:
diffuse_col = v_double()
ambient_col = v_double()
diffuse_col[:] = [0.5, 0.0, 0.0, 0.0]
ambient_col[:] = [0.8, 0.0, 0.0, 0.0]
for o in self.obstacles:
self.robot.setObstacleColor(o.getName(),
diffuse_col,
ambient_col)
except:
pass
time.sleep(0.3)
def update_viewer(self, x, z, control_duration=None, colliding_obstacle=None):
""" Update the viewer to display the state 'x' of the robot
Optionally sleep for 'control_duration'
"""
if self.show_viewer:
cjvals = v_double()
cjvels = v_double()
cjvals_arr = [x[i] for i in xrange(len(x) / 2)]
cjvels_arr = [x[i] for i in xrange(len(x) / 2, len(x))]
cjvals[:] = cjvals_arr
cjvels[:] = cjvels_arr
particle_joint_values = v2_double()
pjv = v_double()
pjv[:] = [z[i] for i in xrange(len(z))]
particle_joint_values.append(pjv)
particle_joint_colors = v2_double()
pja = v_double()
pja[:] = [0.5, 0.5, 0.9, 0.0]
particle_joint_colors.append(pja)
self.robot.updateViewerValues(cjvals,
cjvels,
particle_joint_values,
particle_joint_colors)
for o in self.obstacles:
self.robot.setObstacleColor(o.getName(),
o.getStandardDiffuseColor(),
o.getStandardAmbientColor())
if colliding_obstacle != None:
diffuse_col = v_double()
ambient_col = v_double()
diffuse_col[:] = [0.5, 0.0, 0.0, 0.0]
ambient_col[:] = [0.8, 0.0, 0.0, 0.0]
self.robot.setObstacleColor(colliding_obstacle.getName(),
diffuse_col,
ambient_col)
time.sleep(control_duration)
time.sleep(control_duration)
def simulate_n_steps(self,
xs, us, zs,
control_durations,
x_true,
x_tilde,
x_tilde_linear,
x_estimate,
P_t,
total_reward,
current_step,
n_steps,
deviation_covariances,
estimated_deviation_covariances,
max_num_steps=None):
history_entries = []
terminal_state_reached = False
success = False
collided = False
x_dash = np.copy(x_tilde)
x_dash_linear = np.copy(x_tilde_linear)
x_true_linear = x_true
As, Bs, Vs, Ms, Hs, Ws, Ns = self.get_linear_model_matrices(xs, us, control_durations)
Ls = kalman.compute_gain(As, Bs, self.C, self.D, len(xs) - 1)
logging.info("Simulator: Executing for " + str(n_steps) + " steps")
estimated_states = []
estimated_covariances = []
self.show_nominal_path(xs)
""" Execute the nominal path for n_steps """
for i in xrange(n_steps):
if not terminal_state_reached:
linearization_error = utils.dist(x_dash, x_dash_linear)
history_entries.append(HistoryEntry(current_step,
x_true,
xs[i],
x_true_linear,
x_estimate,
x_dash,
x_dash_linear,
linearization_error,
None,
None,
None,
P_t,
False,
False,
False,
0.0))
current_step += 1
history_entries[-1].set_s_dash_estimated(x_tilde)
""" Calc u_dash from the estimated deviation of the state from the nominal path 'x_tilde'
using the optimal gain L
"""
u_dash = np.dot(Ls[i], x_tilde)
history_entries[-1].set_u_dash(u_dash)
""" Calc the actual control input """
u = u_dash + us[i]
""" Enforce the control constraints on 'u' and 'u_dash' """
if self.enforce_constrol_constraints:
u, u_dash = self.enforce_control_constraints(u, us[i])
history_entries[-1].set_action(u)
history_entries[-1].set_nominal_action(us[i])
""" Apply the control 'u' and propagate the state 'x_true' """
x_true_temp, ce = self.apply_control(x_true,
u,
control_durations[i],
As[i],
Bs[i],
Vs[i],
Ms[i])
""" Calc the linearized next state (used to compare with x_true) """
x_dash_linear_temp = self.get_linearized_next_state(x_dash_linear, u_dash, ce, As[i], Bs[i], Vs[i])
x_true_linear_temp = np.add(x_dash_linear_temp, xs[i+1])
if self.enforce_constraints:
x_true_linear_temp = self.check_constraints(x_true_linear_temp)
discount = np.power(self.discount_factor, current_step - 1)
""" Check if the propagated state collides with an obstacle
If yes, the true state is set to the previous state with 0 velocity.
If not, set the true state to the propagated state
"""
collided = False
in_collision_true_state, colliding_obstacle = self.is_in_collision(x_true, x_true_temp)
if in_collision_true_state:
for j in xrange(len(x_true) / 2, len(x_true)):
x_true[j] = 0.0
logging.info("Simulator: Collision detected. Setting state estimate to the previous state")
total_reward += discount * (-1.0 * self.illegal_move_penalty)
history_entries[-1].set_reward(discount * (-1.0 * self.illegal_move_penalty))
history_entries[-1].set_colliding_obstacle(colliding_obstacle.getName())
history_entries[-1].set_colliding_state(x_true_temp)
collided = True
else:
total_reward += discount * (-1.0 * self.step_penalty)
history_entries[-1].set_reward(discount * (-1.0 * self.illegal_move_penalty))
x_true = x_true_temp
""" Do the same for the linearized true state """
if self.is_in_collision(x_true_linear, x_true_linear_temp)[0]:
for j in xrange(len(x_true_linear) / 2, len(x_true_linear)):
x_true_linear[j] = 0.0
else:
x_true_linear = x_true_linear_temp
""" Calculate the state deviation from the nominal path """
x_dash = np.subtract(x_true, xs[i + 1])
x_dash_linear = np.subtract(x_true_linear , xs[i + 1])
""" Get the end effector position for the true state """
state = v_double()
state[:] = [x_true[j] for j in xrange(len(x_true) / 2)]
ee_position_arr = v_double()
self.robot.getEndEffectorPosition(state, ee_position_arr)
ee_position = np.array([ee_position_arr[j] for j in xrange(len(ee_position_arr))])
logging.info("Simulator: Current end-effector position is " + str(ee_position))
""" Generate an observation for the true state """
z = self.get_observation(x_true, Hs[i + 1], Ns[i + 1], Ws[i + 1])
z_dash = np.subtract(z, zs[i+1])
history_entries[-1].set_observation(z)
""" Update the viewer to display the true state """
self.update_viewer(x_true, z, control_durations[i], colliding_obstacle)
""" Kalman prediction and update """
x_tilde_dash, P_dash = kalman.kalman_predict(x_tilde, u_dash, As[i], Bs[i], P_t, Vs[i], Ms[i])
x_tilde, P_t = kalman.kalman_update(x_tilde_dash,
z_dash,
Hs[i],
P_dash,
Ws[i],
Ns[i])
""" x_estimate_new is the estimated state """
x_estimate_new = x_tilde + xs[i + 1]
""" Enforce the constraints to the estimated state """
if self.enforce_constraints:
x_estimate_new = self.check_constraints(x_estimate_new)
""" Check if the estimated state collides.
If yes, set it to the previous estimate (with velicity 0).
If no, set the true estimate to this estimate
"""
estimate_collided = True
if not self.is_in_collision([], x_estimate_new)[0]:
x_estimate = x_estimate_new
estimate_collided = False
else:
for i in xrange(len(x_estimate) / 2, len(x_estimate)):
x_estimate[i] = 0
estimated_states.append(x_estimate)
estimated_covariances.append(P_t)
""" Check if the end-effector position is terminal. If yes, we're done """
if self.is_terminal(ee_position):
discount = np.power(self.discount_factor, current_step)
history_entries.append(HistoryEntry(current_step,
x_true,
xs[i + 1],
x_true_linear,
x_estimate,
x_dash,
x_dash_linear,
0.0,
None,
None,
z,
P_t,
False,
False,
True,
discount * self.exit_reward))
terminal_state_reached = True
total_reward += discount * self.exit_reward
history_entries[-1].set_linearization_error(utils.dist(x_dash, x_dash_linear))
logging.info("Terminal state reached: reward = " + str(total_reward))
history_entries[-1].set_collided(collided)
history_entries[-1].set_estimate_collided(estimate_collided)
if current_step == max_num_steps:
""" This was the last step -> append final history entry"""
history_entries.append(HistoryEntry(current_step,
x_true,
xs[i + 1],
x_true_linear,
x_estimate,
x_dash,
x_dash_linear,
0.0,
None,
None,
z,
P_t,
False,
False,
False,
0.0))
history_entries[-1].set_linearization_error(utils.dist(x_dash, x_dash_linear))
#total_reward += discount * self.exit_reward
if (collided and self.stop_when_colliding):
break
#print "========================================"
#print "======= Simulation done"
#print "========================================"
return (x_true,
x_tilde,
x_dash_linear,
x_estimate,
P_t,
current_step,
total_reward,
terminal_state_reached,
estimated_states,
estimated_covariances,
history_entries)
def setup_problem(self,
A,
B,
C,
D,
H,
V,
W,
M,
N,
robot,
enforce_control_constraints,
obstacles,
goal_position,
goal_radius,
joint_velocity_limit,
show_viewer,
model_file,
env_file):
self.A = A
self.B = B
self.C = C
self.D = D
self.H = H
self.V = V
self.W = W
self.M = M
self.N = N
self.robot = robot
self.obstacles = obstacles
self.goal_position = goal_position
self.goal_radius = goal_radius
active_joints = v_string()
robot.getActiveJoints(active_joints)
lower_position_constraints = v_double()
upper_position_constraints = v_double()
robot.getJointLowerPositionLimits(active_joints, lower_position_constraints)
robot.getJointUpperPositionLimits(active_joints, upper_position_constraints)
self.lower_position_constraints = [lower_position_constraints[i] for i in xrange(len(lower_position_constraints))]
self.upper_position_constraints = [upper_position_constraints[i] for i in xrange(len(upper_position_constraints))]
velocity_constraints = v_double()
robot.getJointVelocityLimits(active_joints, velocity_constraints)
self.velocity_constraints = [velocity_constraints[i] for i in xrange(len(velocity_constraints))]
self.max_joint_velocities = [joint_velocity_limit for i in xrange(2 * len(active_joints))]
torque_limits = v_double()
robot.getJointTorqueLimits(active_joints, torque_limits)
torque_limits = [torque_limits[i] for i in xrange(len(torque_limits))]
torque_limits.extend([0.0 for i in xrange(len(active_joints))])
self.torque_limits = [torque_limits[i] for i in xrange(len(torque_limits))]
self.enforce_constraints = robot.constraintsEnforced()
self.enforce_constrol_constraints = enforce_control_constraints
self.dynamic_problem = False
self.stop_when_colliding = False
self.show_viewer = show_viewer
active_joints = v_string()
self.robot.getActiveJoints(active_joints)
self.robot_dof = self.robot.getDOF()
if show_viewer:
self.robot.setupViewer(model_file, env_file)
def __init__(self):
self.abs_path = os.path.dirname(os.path.abspath(__file__))
cmd = "rm -rf " + self.abs_path + "/tmp"
popen = subprocess.Popen(cmd, cwd=self.abs_path, shell=True)
popen.wait()
""" Reading the config """
warnings.filterwarnings("ignore")
self.init_serializer()
config = self.serializer.read_config("config_hrf.yaml",
path=self.abs_path)
self.set_params(config)
if self.seed < 0:
"""
Generate a random seed that will be stored
"""
self.seed = np.random.randint(0, 42949672)
np.random.seed(self.seed)
logging_level = logging.WARN
if config['verbose']:
logging_level = logging.DEBUG
logging.basicConfig(format='%(levelname)s: %(message)s',
level=logging_level)
np.set_printoptions(precision=16)
dir = self.abs_path + "/stats/hrf"
tmp_dir = self.abs_path + "/tmp/hrf"
self.utils = Utils()
if not self.init_robot(self.robot_file):
logging.error("HRF: Couldn't initialize robot")
return
if not self.setup_scene(self.environment_file, self.robot):
return
self.clear_stats(dir)
logging.info("Start up simulator")
sim = Simulator()
plan_adjuster = PlanAdjuster()
path_evaluator = PathEvaluator()
path_planner = PathPlanningInterface()
if self.show_viewer_simulation:
self.robot.setupViewer(self.robot_file, self.environment_file)
logging.info("HRF: Generating goal states...")
problem_feasible = False
while not problem_feasible:
print "Creating random scene..."
#self.create_random_obstacles(20)
goal_states = get_goal_states(
"hrf", self.abs_path, self.serializer, self.obstacles,
self.robot, self.max_velocity, self.delta_t, self.start_state,
self.goal_position, self.goal_radius, self.planning_algortihm,
self.path_timeout, self.num_generated_goal_states,
self.continuous_collision, self.environment_file,
self.num_cores)
if len(goal_states) == 0:
logging.error(
"HRF: Couldn't generate any goal states. Problem seems to be infeasible"
)
else:
problem_feasible = True
'''obst = v_obstacle()
obst[:] = self.obstacles
self.robot.addObstacles(obst)'''
logging.info("HRF: Generated " + str(len(goal_states)) +
" goal states")
sim.setup_reward_function(self.discount_factor, self.step_penalty,
self.illegal_move_penalty, self.exit_reward)
path_planner.setup(self.robot, self.obstacles, self.max_velocity,
self.delta_t, self.use_linear_path,
self.planning_algortihm, self.path_timeout,
self.continuous_collision, self.num_cores)
if self.dynamic_problem:
path_planner.setup_dynamic_problem(
self.robot_file, self.environment_file,
self.simulation_step_size, self.num_control_samples,
self.min_control_duration, self.max_control_duration,
self.add_intermediate_states, self.rrt_goal_bias,
self.control_sampler)
A, H, B, V, W, C, D, M_base, N_base = self.problem_setup(
self.delta_t, self.robot_dof)
time_to_generate_paths = 0.0
if check_positive_definite([C, D]):
m_covs = None
if self.inc_covariance == "process":
m_covs = np.linspace(self.min_process_covariance,
self.max_process_covariance,
self.covariance_steps)
elif self.inc_covariance == "observation":
m_covs = np.linspace(self.min_observation_covariance,
self.max_observation_covariance,
self.covariance_steps)
for j in xrange(len(m_covs)):
M = None
N = None
if self.inc_covariance == "process":
""" The process noise covariance matrix """
M = self.calc_covariance_value(self.robot, m_covs[j], M_base)
#M = m_covs[j] * M_base
""" The observation error covariance matrix """
N = self.calc_covariance_value(self.robot,
self.min_observation_covariance,
N_base,
covariance_type='observation')
elif self.inc_covariance == "observation":
M = self.calc_covariance_value(self.robot,
self.min_process_covariance,
M_base)
N = self.calc_covariance_value(self.robot,
m_covs[j],
N_base,
covariance_type='observation')
path_planner.setup_path_evaluator(
A, B, C, D, H, M, N, V, W, self.robot, self.sample_size,
self.obstacles, self.goal_position, self.goal_radius,
self.robot_file, self.environment_file)
sim.setup_problem(A, B, C, D, H, V, W, M, N, self.robot,
self.enforce_control_constraints, self.obstacles,
self.goal_position, self.goal_radius,
self.max_velocity, self.show_viewer_simulation,
self.robot_file, self.environment_file,
self.knows_collision)
path_evaluator.setup(A, B, C, D, H, M, N, V, W, self.robot,
self.sample_size, self.obstacles,
self.goal_position, self.goal_radius,
self.show_viewer_evaluation, self.robot_file,
self.environment_file, self.num_cores)
path_evaluator.setup_reward_function(self.step_penalty,
self.illegal_move_penalty,
self.exit_reward,
self.discount_factor)
plan_adjuster.setup(self.robot, M, H, W, N, C, D,
self.dynamic_problem,
self.enforce_control_constraints)
plan_adjuster.set_simulation_step_size(self.simulation_step_size)
plan_adjuster.set_model_matrices(A, B, V)
if not self.dynamic_problem:
plan_adjuster.set_max_joint_velocities_linear_problem(
np.array(
[self.max_velocity for i in xrange(self.robot_dof)]))
sim.set_stop_when_colliding(self.replan_when_colliding)
if self.dynamic_problem:
path_evaluator.setup_dynamic_problem()
sim.setup_dynamic_problem(self.simulation_step_size)
path_planner.setup_reward_function(self.step_penalty,
self.exit_reward,
self.illegal_move_penalty,
self.discount_factor)
mean_number_planning_steps = 0.0
number_of_steps = 0.0
mean_planning_time = 0.0
num_generated_paths_run = 0.0
successful_runs = 0
num_collisions = 0.0
linearization_error = 0.0
final_states = []
rewards_cov = []
for k in xrange(self.num_simulation_runs):
print "HRF: Run " + str(k + 1)
self.serializer.write_line("log.log", tmp_dir,
"RUN #" + str(k + 1) + " \n")
current_step = 0
x_true = np.array([
self.start_state[m] for m in xrange(len(self.start_state))
])
x_estimated = np.array([
self.start_state[m] for m in xrange(len(self.start_state))
])
#x_estimated[0] += 0.2
#x_estimated[0] = 0.4
#x_predicted = self.start_state
P_t = np.array([[0.0 for i in xrange(2 * self.robot_dof)]
for i in xrange(2 * self.robot_dof)])
P_ext_t = np.array([[0.0 for i in xrange(2 * self.robot_dof)]
for i in xrange(2 * self.robot_dof)])
deviation_covariance = np.array(
[[0.0 for i in xrange(2 * self.robot_dof)]
for i in xrange(2 * self.robot_dof)])
estimated_deviation_covariance = np.array(
[[0.0 for i in xrange(2 * self.robot_dof)]
for i in xrange(2 * self.robot_dof)])
total_reward = 0.0
terminal = False
last_x_true = None
"""
Obtain a nominal path
"""
print "plan"
path_planner.set_start_and_goal(x_estimated, goal_states,
self.goal_position,
self.goal_radius)
t0 = time.time()
(xs, us, zs, control_durations, num_generated_paths, objective,
state_covariances, deviation_covariances,
estimated_deviation_covariances, mean_gen_times,
mean_eval_times, total_gen_times,
total_eval_times) = path_planner.plan_and_evaluate_paths(
1, 0, current_step, self.evaluation_horizon, P_t,
deviation_covariance, estimated_deviation_covariance, 0.0)
print "objective " + str(objective)
while True:
print "current step " + str(current_step)
'''if current_step == 7:
x_true = np.array([last_x_true[k] for k in xrange(len(last_x_true))])
for k in xrange(len(last_x_true) / 2, len(last_x_true)):
last_x_true[k] = 0.0'''
"""
Predict system state at t+1 using nominal path
"""
""" Get state matrices """
#As, Bs, Vs, Ms, Hs, Ws, Ns = sim.get_linear_model_matrices(xs, us, control_durations)
As, Bs, Vs, Ms, Hs, Ws, Ns = sim.get_linear_model_matrices(
[x_estimated], [us[0]], control_durations)
""" Predict using EKF """
(x_predicted_temp, P_predicted) = kalman.predict_state(
self.robot, x_estimated, us[0], control_durations[0],
self.simulation_step_size, As[0], Bs[0], Vs[0], Ms[0],
P_ext_t, self.dynamic_problem)
""" Make sure x_predicted fulfills the constraints """
if self.enforce_constraints:
x_predicted_temp = sim.check_constraints(
x_predicted_temp)
predicted_collided = True
if not sim.is_in_collision([], x_predicted_temp)[0]:
x_predicted = x_predicted_temp
predicted_collided = False
else:
print "X_PREDICTED COLLIDES!"
x_predicted = x_estimated
for l in xrange(
len(x_predicted) / 2, len(x_predicted)):
x_predicted[l] = 0
last_x_true = np.array(
[x_true[k] for k in xrange(len(x_true))])
"""
Execute path for 1 time step
"""
(x_true, x_tilde, x_tilde_linear, x_estimated_dash, z, P_t,
current_step, total_reward, terminal, estimated_s,
estimated_c, history_entries) = sim.simulate_n_steps(
xs,
us,
zs,
control_durations,
x_true,
x_estimated,
P_t,
total_reward,
current_step,
1,
0.0,
0.0,
max_num_steps=self.max_num_steps)
history_entries[-1].set_best_reward(objective)
"""
Process history entries
"""
try:
deviation_covariance = deviation_covariances[
len(history_entries) - 1]
estimated_deviation_covariance = estimated_deviation_covariances[
len(history_entries) - 1]
except:
print "what: len(deviation_covariances) " + str(
len(deviation_covariances))
print "len(history_entries) " + str(
len(history_entries))
print "len(xs) " + str(len(xs))
history_entries[0].set_replanning(True)
for l in xrange(len(history_entries)):
try:
history_entries[l].set_estimated_covariance(
state_covariances[l])
except:
print "l " + str(l)
print "len(state_covariances) " + str(
len(state_covariances))
if history_entries[l].collided:
num_collisions += 1
linearization_error += history_entries[
l].linearization_error
if (current_step == self.max_num_steps) or terminal:
history_entries[len(history_entries) -
2].set_best_reward(objective)
history_entries[-1].set_best_reward(None)
for l in xrange(len(history_entries)):
history_entries[l].serialize(tmp_dir, "log.log")
final_states.append(history_entries[-1].x_true)
if terminal:
print "Terminal state reached"
successful_runs += 1
break
"""
Plan new trajectories from predicted state
"""
path_planner.set_start_and_goal(x_predicted, goal_states,
self.goal_position,
self.goal_radius)
t0 = time.time()
paths = path_planner.plan_paths(
self.num_paths,
0,
planning_timeout=self.timeout,
min_num_paths=1)
mean_planning_time += time.time() - t0
mean_number_planning_steps += 1.0
num_generated_paths_run += len(paths)
"""
Filter update
"""
(x_estimated_temp,
P_ext_t) = kalman.filter_update(x_predicted, P_predicted,
z, Hs[0], Ws[0], Ns[0])
""" Make sure x_estimated fulfills the constraints """
if self.enforce_constraints:
x_estimated_temp = sim.check_constraints(
x_estimated_temp)
in_collision, colliding_obstacle = sim.is_in_collision(
[], x_estimated_temp)
if in_collision:
history_entries[-1].set_estimate_collided(True)
for l in xrange(
len(x_estimated) / 2, len(x_estimated)):
x_estimated[l] = 0
elif history_entries[-1].collided and self.knows_collision:
for l in xrange(
len(x_estimated) / 2, len(x_estimated)):
x_estimated[l] = 0
else:
x_estimated = x_estimated_temp
history_entries[-1].set_estimate_collided(False)
history_entries[-1].serialize(tmp_dir, "log.log")
"""
Adjust plan
"""
t0 = time.time()
(xs_adj, us_adj, zs_adj, control_durations_adj,
adjusted) = plan_adjuster.adjust_plan(
self.robot, (xs, us, zs, control_durations),
x_estimated, P_t)
if not adjusted:
final_states.append(history_entries[-1].x_true)
print "current step " + str(current_step)
raise ValueError("Raised")
break
if self.show_viewer_simulation:
sim.update_viewer(
x_true,
x_estimated,
z,
control_duration=0.03,
colliding_obstacle=sim.colliding_obstacle)
"""
Evaluate the adjusted plan and the planned paths
"""
#if self.is_terminal(xs_adj[-1]) and not len(xs_adj)==1:
if not len(xs_adj) <= 1:
"""
Only evaluate the adjusted path if it's > 1
"""
paths.extend(
[[xs_adj, us_adj, zs_adj, control_durations_adj]])
if len(paths) == 0:
"""
Running out of paths
"""
print "Error: Couldn't generate an evaluate any paths"
final_states.append(history_entries[-1].x_true)
break
(path_index, xs, us, zs, control_durations, objective,
state_covariances, deviation_covariances,
estimated_deviation_covariances
) = path_evaluator.evaluate_paths(paths, P_t,
current_step)
mean_planning_time += time.time() - t0
if path_index == None:
"""
We couldn't evaluate any paths
"""
final_states.append(history_entries[-1].x_true)
break
if self.show_viewer_simulation:
self.visualize_paths(self.robot, [xs])
rewards_cov.append(total_reward)
self.serializer.write_line(
"log.log", tmp_dir, "Reward: " + str(total_reward) + " \n")
self.serializer.write_line("log.log", tmp_dir, "\n")
number_of_steps += current_step
mean_planning_time_per_step = mean_planning_time / mean_number_planning_steps
mean_planning_time /= self.num_simulation_runs
mean_num_generated_paths_step = num_generated_paths_run / mean_number_planning_steps
""" Calculate the distance to goal area
"""
ee_position_distances = []
for state in final_states:
joint_angles = v_double()
joint_angles[:] = [state[y] for y in xrange(len(state) / 2)]
ee_position = v_double()
self.robot.getEndEffectorPosition(joint_angles, ee_position)
ee_position = np.array(
[ee_position[y] for y in xrange(len(ee_position))])
dist = np.linalg.norm(
np.array(self.goal_position - ee_position))
if dist < self.goal_radius:
dist = 0.0
ee_position_distances.append(dist)
logging.info("HRF: Done. total_reward is " + str(total_reward))
try:
n, min_max, mean_distance_to_goal, var, skew, kurt = scipy.stats.describe(
np.array(ee_position_distances))
except:
print ee_position_distances
self.serializer.write_line("log.log", tmp_dir,
"################################# \n")
self.serializer.write_line(
"log.log", tmp_dir,
"inc_covariance: " + str(self.inc_covariance) + "\n")
if self.inc_covariance == "process":
self.serializer.write_line(
"log.log", tmp_dir,
"Process covariance: " + str(m_covs[j]) + " \n")
self.serializer.write_line(
"log.log", tmp_dir, "Observation covariance: " +
str(self.min_observation_covariance) + " \n")
elif self.inc_covariance == "observation":
self.serializer.write_line(
"log.log", tmp_dir, "Process covariance: " +
str(self.min_process_covariance) + " \n")
self.serializer.write_line(
"log.log", tmp_dir,
"Observation covariance: " + str(m_covs[j]) + " \n")
mean_linearization_error = linearization_error / number_of_steps
number_of_steps /= self.num_simulation_runs
self.serializer.write_line(
"log.log", tmp_dir,
"Mean number of steps: " + str(number_of_steps) + " \n")
self.serializer.write_line(
"log.log", tmp_dir,
"Mean number of generated paths per step: " +
str(mean_num_generated_paths_step) + " \n")
self.serializer.write_line(
"log.log", tmp_dir, "Mean num collisions per run: " +
str(float(num_collisions) / float(self.num_simulation_runs)) +
" \n")
self.serializer.write_line(
"log.log", tmp_dir, "Average distance to goal area: " +
str(mean_distance_to_goal) + " \n")
self.serializer.write_line(
"log.log", tmp_dir,
"Num successes: " + str(successful_runs) + " \n")
print "succ " + str(
(100.0 / self.num_simulation_runs) * successful_runs)
self.serializer.write_line(
"log.log", tmp_dir, "Percentage of successful runs: " + str(
(100.0 / self.num_simulation_runs) * successful_runs) +
" \n")
self.serializer.write_line(
"log.log", tmp_dir, "Mean planning time per run: " +
str(mean_planning_time) + " \n")
self.serializer.write_line(
"log.log", tmp_dir, "Mean planning time per planning step: " +
str(mean_planning_time_per_step) + " \n")
n, min_max, mean, var, skew, kurt = scipy.stats.describe(
np.array(rewards_cov))
print "mean_rewards " + str(mean)
#plt.plot_histogram_from_data(rewards_cov)
#sleep
self.serializer.write_line("log.log", tmp_dir,
"Mean rewards: " + str(mean) + " \n")
self.serializer.write_line("log.log", tmp_dir,
"Reward variance: " + str(var) + " \n")
self.serializer.write_line(
"log.log", tmp_dir, "Mean linearisation error: " +
str(mean_linearization_error) + " \n")
self.serializer.write_line(
"log.log", tmp_dir,
"Reward standard deviation: " + str(np.sqrt(var)) + " \n")
self.serializer.write_line("log.log", tmp_dir,
"Seed: " + str(self.seed) + " \n")
cmd = "mv " + tmp_dir + "/log.log " + dir + "/log_hrf_" + str(
m_covs[j]) + ".log"
os.system(cmd)
cmd = "cp " + self.abs_path + "/config_hrf.yaml " + dir
os.system(cmd)
if not os.path.exists(dir + "/environment"):
os.makedirs(dir + "/environment")
cmd = "cp " + self.abs_path + "/" + str(
self.environment_file) + " " + str(dir) + "/environment"
os.system(cmd)
if not os.path.exists(dir + "/model"):
os.makedirs(dir + "/model")
cmd = "cp " + self.abs_path + "/" + self.robot_file + " " + dir + "/model"
os.system(cmd)
print "Done."
def _construct(self, robot, obstacles):
path_planner2 = None
if not self.dynamic_problem:
path_planner2 = libpath_planner.PathPlanner(
robot, self.delta_t, self.continuous_collision,
self.max_velocity, 1.0, self.use_linear_path, self.verbose,
self.planning_algorithm)
path_planner2.setup()
else:
path_planner2 = libdynamic_path_planner.DynamicPathPlanner(
robot, self.verbose)
path_planner2.setupMotionValidator(self.continuous_collision)
logging.info(
"PathPlanningInterface: Set up motion validator. Setting kinematics..."
)
logging.info(
"PathPlanningInterface: Kinematics set. Running setup...")
path_planner2.setup(self.simulation_step_size, self.delta_t)
path_planner2.setControlSampler(self.control_sampler)
num_control_samples = libdynamic_path_planner.v_int()
num_control_samples[:] = [self.num_control_samples]
path_planner2.setNumControlSamples(num_control_samples)
path_planner2.setRRTGoalBias(self.rrt_goal_bias)
min_max_control_duration = libdynamic_path_planner.v_int()
min_max_control_duration[:] = [
self.min_control_duration, self.max_control_duration
]
path_planner2.setMinMaxControlDuration(min_max_control_duration)
path_planner2.addIntermediateStates(self.add_intermediate_states)
logging.info("PathPlanningInterface: Path planner setup")
path_planner2.setObstacles(obstacles)
goal_states = v2_double()
gs = []
'''
Check if the start state is valid
'''
ss_vec = v_double()
ss_vec[:] = self.start_state
if not path_planner2.isValid(ss_vec):
logging.warn("Path planning interface: Start state not valid")
return [], [], [], [], False
'''
Check of the goal states are valid
'''
for i in xrange(len(self.goal_states)):
goal_state = v_double()
goal_state[:] = [
self.goal_states[i][j]
for j in xrange(len(self.goal_states[i]))
]
if path_planner2.isValid(goal_state):
gs.append(goal_state)
if len(gs) == 0:
logging.warn("PathPlanningInterface: No valid goal states")
return [], [], [], [], False
goal_states[:] = gs
ee_goal_position = v_double()
ee_goal_position[:] = self.ee_goal_position
path_planner2.setGoalStates(goal_states, ee_goal_position,
self.ee_goal_threshold)
start_state = v_double()
v = [self.start_state[i] for i in xrange(len(self.start_state))]
start_state[:] = v
logging.info("PathPlanningInterface: Solve...")
xs_temp = path_planner2.solve(start_state, self.path_timeout)
xs = []
us = []
zs = []
control_durations = []
for i in xrange(len(xs_temp)):
xs.append([xs_temp[i][j] for j in xrange(0, 2 * self.robot_dof)])
us.append([
xs_temp[i][j]
for j in xrange(2 * self.robot_dof, 3 * self.robot_dof)
])
zs.append([
xs_temp[i][j]
for j in xrange(3 * self.robot_dof, 5 * self.robot_dof)
])
control_durations.append(xs_temp[i][5 * self.robot_dof])
'''if self.dynamic_problem:
all_states = v2_double()
print "getting all states"
path_planner2.getAllStates(all_states)
print "got all states " + str(len(all_states))
plot_states_states = [np.array(all_states[i][0:3]) for i in xrange(len(all_states))]
plot_states_velocities = [np.array(all_states[i][3:6]) for i in xrange(len(all_states))]
from plot import plot_3d_points
scale = [-np.pi - 0.01, np.pi + 0.01]
scale2 = [-11, 11]
plot_3d_points(np.array(plot_states_states),
scale,
scale,
scale)
plot_3d_points(np.array(plot_states_velocities),
scale2,
scale2,
scale2)'''
return xs, us, zs, control_durations, True
def get_end_effector_position(robot, state):
s = v_double()
s[:] = [state[i] for i in xrange(len(state))]
ee_position = v_double()
robot.getEndEffectorPosition(s, ee_position)
return np.array([ee_position[i] for i in xrange(len(ee_position))])
def adjust_plan(self,
robot,
plan,
x_estimated,
P_t):
xs = [plan[0][i] for i in xrange(1, len(plan[0]))]
us = [plan[1][i] for i in xrange(1, len(plan[1]))]
zs = [plan[2][i] for i in xrange(1, len(plan[2]))]
control_durations = [plan[3][i] for i in xrange(1, len(plan[3]))]
As, Bs, Vs, Ms, Hs, Ws, Ns = self.get_linear_model_matrices(robot,
xs,
us,
control_durations)
Ls = kalman.compute_gain(As, Bs, self.C, self.D, len(xs) - 1)
xs_adjusted = []
us_adjusted = []
zs_adjusted = []
xs_adjusted.append(xs[0])
zs_adjusted.append(zs[0])
try:
x_tilde = x_estimated - xs[0]
except Exception as e:
print e
print "==================="
print "x_estimated " + str(x_estimated)
print "xs[0] " + str(xs[0])
print "xs " + str(xs)
return (None, None, None, None, True)
for i in xrange(len(xs) - 1):
x_predicted = np.array([xs[i][k] for k in xrange(len(xs[i]))])
u = np.dot(Ls[i], x_estimated - x_predicted) + us[i]
if self.control_constraints_enforced:
u = self.enforce_control_constraints(u)
if self.dynamic_problem:
current_state = v_double()
current_state[:] = [xs_adjusted[i][j] for j in xrange(len(xs_adjusted[i]))]
control = v_double()
control[:] = u
control_error = v_double()
control_error[:] = [0.0 for k in xrange(len(u))]
result = v_double()
robot.propagate(current_state,
control,
control_error,
self.simulation_step_size,
control_durations[i],
result)
xs_adjusted.append(np.array([result[k] for k in xrange(len(result))]))
us_adjusted.append(np.array([u[k] for k in xrange(len(u))]))
zs_adjusted.append(np.array([result[k] for k in xrange(len(result))]))
#print "xs_adjusted: " + str(np.array([result[k] for k in xrange(len(result))]))
#print "xs_prior: " + str(xs[i + 1])
else:
current_state = np.array([xs_adjusted[i][j] for j in xrange(len(xs_adjusted[i]))])
result = np.dot(As[i], current_state) + np.dot(Bs[i], u)
xs_adjusted.append(np.array([result[k] for k in xrange(len(result))]))
us_adjusted.append(np.array([u[k] for k in xrange(len(u))]))
zs_adjusted.append(np.array([result[k] for k in xrange(len(result))]))
u_dash = u - us[i]
z_dash = np.array([0.0 for k in xrange(len(zs_adjusted[-1]))])
"""
Maximum likelikhood observation
"""
""" Kalman prediction and update """
x_tilde_dash, P_dash = kalman.kalman_predict(x_tilde, u_dash, As[i], Bs[i], P_t, Vs[i], Ms[i])
x_tilde, P_t = kalman.kalman_update(x_tilde_dash,
z_dash,
Hs[i],
P_dash,
Ws[i],
Ns[i])
x_estimated = x_tilde + xs[i + 1]
#x_estimated = x_tilde + xs_adjusted[-1]
us_adjusted.append(np.array([0.0 for i in xrange(len(us[0]))]))
control_durations_adjusted = [control_durations[i] for i in xrange(len(control_durations))]
control_durations_adjusted.append(0.0)
return (xs_adjusted, us_adjusted, zs_adjusted, control_durations_adjusted, True)
