def show_state_distribution(self, model_file, env_file):
self.robot.setupViewer(model_file, env_file)
M = 30.0 * np.identity(2 * self.robot_dof)
active_joints = v_string()
self.robot.getActiveJoints(active_joints)
self.robot_dof = len(active_joints)
#x = [0.0, -np.pi / 2.0, 0.0, 0.0, 0.0, 0.0]
states = []
for z in xrange(2000):
u = [70.0, 70.0, 70.0, 0.0, 0.0, 0.0]
current_state = v_double()
current_state[:] = x
control = v_double()
control[:] = u
control_error = v_double()
ce = self.sample_control_error(M)
#ce = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
control_error[:] = ce
x = None
for k in xrange(1):
result = v_double()
self.robot.propagate(current_state, control, control_error,
self.simulation_step_size, self.delta_t,
result)
x = [result[i] for i in xrange(len(result))]
current_state[:] = x
print x
states.append(np.array(x[3:6]))
x = [0.0, -np.pi / 2.0, 0.0, 0.0, 0.0, 0.0]
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()
particle_joint_colors = v2_double()
self.robot.updateViewerValues(cjvals, cjvels,
particle_joint_values,
particle_joint_values)
from plot import plot_3d_points
mins = []
maxs = []
x_min = min([states[i][0] for i in xrange(len(states))])
x_max = max([states[i][0] for i in xrange(len(states))])
y_min = min([states[i][1] for i in xrange(len(states))])
y_max = max([states[i][1] for i in xrange(len(states))])
z_min = min([states[i][2] for i in xrange(len(states))])
z_max = max([states[i][2] for i in xrange(len(states))])
scale = [-0.2, 0.2]
plot_3d_points(np.array(states),
x_scale=[x_min, x_max],
y_scale=[y_min, y_max],
z_scale=[z_min, z_max])
sleep
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 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 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 transform_ee_position(robot, position):
"""
Transform position to first joint frame
"""
active_joints = v_string()
robot.getActiveJoints(active_joints)
joint_origins = v2_double()
robot.getJointOrigin(active_joints, joint_origins)
joint_origin_first_joint = [joint_origins[0][i] for i in xrange(3)]
position = [position[i] - joint_origin_first_joint[i] for i in xrange(len(position))]
return np.array(position)
def transform_goal(self, goal_position):
"""
Transform goal position to first joint frame
"""
active_joints = v_string()
self.robot.getActiveJoints(active_joints)
joint_origins = v2_double()
self.robot.getJointOrigin(active_joints, joint_origins)
joint_origin_first_joint = [joint_origins[0][i] for i in xrange(3)]
goal_position = [goal_position[i] - joint_origin_first_joint[i] for i in xrange(len(goal_position))]
return goal_position
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 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 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 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 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 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 show_state_distribution(self, model_file, env_file):
self.robot.setupViewer(model_file, env_file)
M = 30.0 * np.identity(2 * self.robot_dof)
active_joints = v_string()
self.robot.getActiveJoints(active_joints)
self.robot_dof = len(active_joints)
#x = [0.0, -np.pi / 2.0, 0.0, 0.0, 0.0, 0.0]
states = []
for z in xrange(2000):
u = [70.0, 70.0, 70.0, 0.0, 0.0, 0.0]
current_state = v_double()
current_state[:] = x
control = v_double()
control[:] = u
control_error = v_double()
ce = self.sample_control_error(M)
#ce = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
control_error[:] = ce
x = None
for k in xrange(1):
result = v_double()
self.robot.propagate(current_state,
control,
control_error,
self.simulation_step_size,
self.delta_t,
result)
x = [result[i] for i in xrange(len(result))]
current_state[:] = x
print x
states.append(np.array(x[3:6]))
x = [0.0, -np.pi/ 2.0, 0.0, 0.0, 0.0, 0.0]
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()
particle_joint_colors = v2_double()
self.robot.updateViewerValues(cjvals,
cjvels,
particle_joint_values,
particle_joint_values)
from plot import plot_3d_points
mins = []
maxs = []
x_min = min([states[i][0] for i in xrange(len(states))])
x_max = max([states[i][0] for i in xrange(len(states))])
y_min = min([states[i][1] for i in xrange(len(states))])
y_max = max([states[i][1] for i in xrange(len(states))])
z_min = min([states[i][2] for i in xrange(len(states))])
z_max = max([states[i][2] for i in xrange(len(states))])
scale = [-0.2, 0.2]
plot_3d_points(np.array(states),
x_scale = [x_min, x_max],
y_scale = [y_min, y_max],
z_scale= [z_min, z_max])
sleep
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 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, knows_collision):
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.control_constraints_enforced = 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()
self.knows_collision = knows_collision
if show_viewer:
self.robot.setupViewer(model_file, env_file)