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)]
print previous_state
print state
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]]):
print "return true"
return True, obstacle
return False, None
else:
for obstacle in collidable_obstacles:
if obstacle.inCollisionDiscrete(collision_objects_goal):
return True, None
return False, None
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 show_nominal_path(self, path):
""" Shows the nominal path in the viewer """
self.environment.removePermanentParticles()
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.environment.plotPermanentParticles(particle_joint_values,
particle_joint_colors)
def show_nominal_path(self, path):
""" Shows the nominal path in the viewer """
self.environment.removePermanentParticles()
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.environment.plotPermanentParticles(particle_joint_values,
particle_joint_colors)
def prog(joint_angles, sensor_name):
joint_angles_v = v_double()
joint_velocities = v_double()
particle_joint_values = v2_double()
joint_angles_v[:] = [joint_angles[i] for i in xrange(len(joint_angles))]
joint_velocities[:] = [0.0, 0.0, 0.0]
env.getRobot().setState(joint_angles_v, joint_velocities)
env.transformSensorSensorLink(joint_angles_v, sensor_name)
env.updateRobotValues(joint_angles_v,
joint_angles_v,
particle_joint_values,
particle_joint_values)
time.sleep(1)
env.getSensorManager().activateSensor(sensor_name)
time.sleep(1)
env.drawBoxes()
time.sleep(1)
env.getSensorManager().deactivateSensor(sensor_name)
time.sleep(1.0)
def init_environment(self):
self.environment = Environment()
self.environment.setupEnvironment(self.environment_file)
self.environment.loadRobotFromURDF(self.robot_file)
self.robot = self.environment.getRobot()
self.robot_dof = self.robot.getDOF()
goal_area = v_double()
self.environment.getGoalArea(goal_area)
self.goal_position = [goal_area[i] for i in xrange(0, 3)]
self.goal_radius = goal_area[3]
self.environment.showViewer()
return True
def test_collision(env):
robot = env.getRobot()
collision_manager = env.getCollisionManager()
joint_angles_v = v_double()
joint_angles_v_2 = v_double()
joint_angles_v[:] = [0.2, 0.0, 0.0]
joint_angles_v_2[:] = [0.2, 0.0, 0.0]
particle_joint_values = v2_double()
env.updateRobotValues(joint_angles_v,
joint_angles_v,
particle_joint_values,
particle_joint_values)
robot_collision_objects_start = robot.createRobotCollisionObjects(joint_angles_v)
robot_collision_objects_goal = robot.createRobotCollisionObjects(joint_angles_v_2)
for i in xrange(len(robot_collision_objects_start)):
in_collision = collision_manager.inCollisionContinuousEnvironment([robot_collision_objects_start[i],
robot_collision_objects_goal[i]])
print in_collision
time.sleep(100)
def init_environment(self):
self.environment = Environment()
self.environment.setupEnvironment(self.environment_file)
self.environment.loadRobotFromURDF(self.robot_file)
self.robot = self.environment.getRobot()
self.robot_dof = self.robot.getDOF()
goal_area = v_double()
self.environment.getGoalArea(goal_area)
self.goal_position = [goal_area[i] for i in xrange(0, 3)]
self.goal_radius = goal_area[3]
self.environment.showViewer()
return True
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)]
print previous_state
print state
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]
]):
print "return true"
return True, obstacle
return False, None
else:
for obstacle in collidable_obstacles:
if obstacle.inCollisionDiscrete(collision_objects_goal):
return True, None
return False, None
def play_states(self, states, col, particles):
for i in xrange(len(states)):
cjvals = v_double()
cjvels = v_double()
print "states[i] " + str(states[i])
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)
print "updating robot values"
self.environment.updateRobotValues(cjvals, cjvels,
particle_joint_values,
particle_joint_colors)
print "updated robot values"
'''for o in self.obstacles:
self.robot.setObstacleColor(o.getName(),
o.getStandardDiffuseColor(),
o.getStandardAmbientColor()) '''
self.environment.setKinBodiesDefaultColor()
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 play_states(self, states, col, particles):
for i in xrange(len(states)):
cjvals = v_double()
cjvels = v_double()
print "states[i] " + str(states[i])
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)
print "updating robot values"
self.environment.updateRobotValues(cjvals,
cjvels,
particle_joint_values,
particle_joint_colors)
print "updated robot values"
'''for o in self.obstacles:
self.robot.setObstacleColor(o.getName(),
o.getStandardDiffuseColor(),
o.getStandardAmbientColor()) '''
self.environment.setKinBodiesDefaultColor()
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 propagate(env, sensor_name):
current_state = v_double()
control_input = v_double()
control_error = v_double()
simulation_step_size = 0.0001
duration = 0.003
result = v_double()
robot_dof_values = v_double()
robot_dof_values_start = v_double()
env.getRobotDOFValues(robot_dof_values)
print "len robot_dof_values " + str(len(robot_dof_values))
#cs = [0.0 for i in xrange(len(robot_dof_values) * 2)]
cv = [0.0 for i in xrange(len(robot_dof_values))]
'''cv[0] = 0.3
cv[1] = -0.3
cv[4] = -0.3
cv[5] = 0.3
cv[6] = 0.3
cv[7] = -0.3
cv[8] = 0.3
cv[9] = -0.3
cv[10] = 0.3
cv[11] = -0.3
cv[12] = -1.5
cv[13] = 1.5
cv[14] = -1.5
cv[15] = 1.5
cv[16] = -1.5
cv[17] = 1.5'''
cs = [cv[i] for i in xrange(len(cv))]
cs.extend([0.0 for i in xrange(len(robot_dof_values))])
#cv[2] = 0.0
current_state[:] = cs
control_input[:] = [0.0 for i in xrange(len(robot_dof_values))]
control_error[:] = [0.0 for i in xrange(len(robot_dof_values))]
robot = env.getRobot()
#control_input[1] = 1.0
robot_dof_values[:] = cv
robot_dof_values_start[:] = cv
env.setRobotDOFValues(robot_dof_values)
while True:
#print "propagating"
robot_dof_values_start[:] = [current_state[i] for i in xrange(len(current_state) / 2)]
robot.propagate(current_state,
control_input,
control_error,
simulation_step_size,
duration,
result)
robot_dof_values[:] = [result[i] for i in xrange(len(result) / 2)]
t0 = time.time()
collides = env.robotCollidesContinuous(robot_dof_values_start,robot_dof_values)
t1 = time.time() - t0
if collides.in_collision:
if collides.contact_body_name == "front_left_end_effector" or collides.contact_body_name == "front_left_tibia":
#control_input[1] = -50.0
body_point = v_double()
body_name = "front_left_end_effector"
world_normal = v_double()
body_point[:] = [0.0, 0.0, 0.0]
world_normal[:] = [0.0, 0.0, 1.0]
robot.propagate_constraints(current_state,
control_input,
control_error,
simulation_step_size,
duration,
body_name,
body_point,
world_normal,
result)
print [result[i] for i in xrange(len(result))]
print "prop"
sensors = v_string()
sensors[:] = [sensor_file]
env.loadSensors(sensors)
env.showViewer()
env.getSensorManager()
env.loadRobotFromURDF("model/block_model.urdf")
#dof_values = v_double()
#dof_values[:] = [1.5778636567,-3.28698057487e-06,4.93129297073e-06,-0.028272851672]
#env.setRobotDOFValues(dof_values)
#time.sleep(100)
env.getRobot().setGravityConstant(9.81)
#env.transformSensorToSensorLink(sensor_name)
env.initOctree(0.1)
robot_dof_values = v_double()
env.getRobotDOFValues(robot_dof_values)
robot_dof_values_arr = [robot_dof_values[i] for i in xrange(len(robot_dof_values))]
propagate(env, sensor_name)
time.sleep(100)
new_robot_dof_values[:] = robot_dof_values_arr
env.setRobotDOFValues(new_robot_dof_values)
time.sleep(3)
robot_trans = v_double()
robot_rot = v_double()
robot_trans[:] = [0.0, 0.0, 0.0]
robot_rot[:] = [0.4, 0.4, 0.4]
env.setRobotTransform(robot_trans, robot_rot)
time.sleep(3)
