def reset(self):
super(DishRackEnv, self).reset()
self.rack_pos[0] = self.np_random.uniform(rack_lower[0], rack_upper[0])
self.rack_pos[1] = self.np_random.uniform(rack_lower[1], rack_upper[1])
self.rack_rot[0] = self.np_random.uniform(rack_lower[2], rack_upper[2])
vrep.simxSetObjectPosition(self.cid, self.rack_handle, -1,
self.rack_pos, vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(self.cid, self.rack_handle,
self.rack_rot_ref, self.rack_rot,
vrep.simx_opmode_blocking)
trg_rot = catch_errors(
vrep.simxGetObjectOrientation(self.cid, self.target_handle, -1,
vrep.simx_opmode_blocking))
vrep.simxSetObjectOrientation(self.cid, self.mv_trg_handle, -1,
trg_rot, vrep.simx_opmode_blocking)
if self.vis_mode:
# OTHER POSES
stand_height_diff = np.append([0, 0],
self.np_random.uniform(
-self.max_height_displacement,
self.max_height_displacement, 1))
vrep.simxSetObjectPosition(self.cid, self.stand_h, -1,
self.init_stand_pos + stand_height_diff,
vrep.simx_opmode_blocking)
self.randomise_domain()
return self._get_obs()
def add_multiple_objects(cid, nb_obj):
object_name_list = []
object_handle_list = []
object_number = nb_obj
# object_list = ['imported_part_0','imported_part_1','imported_part_2','imported_part_3','imported_part_4','imported_part_5','imported_part_6','imported_part_7']
object_list = [
'imported_part_0', 'imported_part_1', 'imported_part_2',
'imported_part_3', 'imported_part_4', 'imported_part_5'
]
for i in range(object_number):
object_name = random.choice(object_list)
object_list.remove(object_name)
object_name_list.append(object_name)
print('Adding %s' % object_name)
res, object_handle = vrep.simxGetObjectHandle(
cid, object_name, vrep.simx_opmode_oneshot_wait)
object_handle_list.append(object_handle)
object_pos = [0, 0, 0.3]
a = random.uniform(-90, 90)
b = random.uniform(-90, 90)
g = random.uniform(-90, 90)
object_angle = [a, b, g]
vrep.simxSetObjectPosition(cid, object_handle, -1, object_pos,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectOrientation(cid, object_handle, -1, object_angle,
vrep.simx_opmode_oneshot_wait)
return object_name_list, object_handle_list
def setPosition(self, stateVector=None):
# stateVector = [x, y, theta]
z0 = 0.1587
if stateVector == None:
x0 = self.xi.x
y0 = self.xi.y
theta0 = self.xi.theta
elif len(stateVector) == 3:
x0 = stateVector[0]
y0 = stateVector[1]
theta0 = stateVector[2]
self.xi.x = x0
self.xi.y = y0
self.xi.theta = theta0
else:
raise Exception('Argument error!')
if self.scene.vrepConnected == False:
return
vrep.simxSetObjectPosition(self.scene.clientID, self.robotHandle, -1,
[x0, y0, z0], vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(self.scene.clientID, self.robotHandle,
-1, [0, 0, theta0],
vrep.simx_opmode_oneshot)
message = "Robot #" + str(self.index) + "'s pose is set to "
message += "[{0:.3f}, {1:.3f}, {2:.3f}]".format(x0, y0, theta0)
self.scene.log(message)
def render_chair(self, table, position, offset):
x, chair = vrep.simxLoadModel(self.clientID,
self.models_path + "dining chair.ttm", 0,
vrep.simx_opmode_blocking)
x = vrep.simxSetObjectParent(self.clientID, chair, table, True,
vrep.simx_opmode_blocking)
offset_z = math.fabs(
vrep.simxGetObjectFloatParameter(
self.clientID, chair, vrep.sim_objfloatparam_modelbbox_min_z,
vrep.simx_opmode_blocking)[1])
vrep.simxSetObjectPosition(
self.clientID, chair, -1,
(position[0] + offset[0], position[1] + offset[1],
position[2] + offset_z + offset[2]), vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(self.clientID, chair, -1,
(offset[3], offset[4], offset[5]),
vrep.simx_opmode_oneshot)
#returnCode, prop = vrep.simxGetModelProperty(self.clientID,chair,vrep.simx_opmode_blocking)
# print (prop, prop + vrep.sim_modelproperty_not_respondable + vrep.sim_modelproperty_not_collidable)
#returnCode2 = vrep.simxSetModelProperty(self.clientID, chair, prop + vrep.sim_modelproperty_not_respondable, vrep.simx_opmode_blocking)
#returnCode3, prop = vrep.simxGetModelProperty(self.clientID,chair,vrep.simx_opmode_blocking)
# print (returnCode, returnCode2, returnCode3, prop)
# vrep.simxSetModelProperty(self.clientID,chair,vrep.sim_modelproperty_not_collidable,vrep.simx_opmode_oneshot)
return chair
def restartScenario(self):
inputBuffer = bytearray()
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
self.clientID, 'suctionPad', vrep.sim_scripttype_childscript,
'sysCall_cleanup', [], [], [], inputBuffer,
vrep.simx_opmode_blocking)
errorCode1, handleJoint = vrep.simxGetObjectHandle(
self.clientID, 'm_Sphere', vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(self.clientID, handleJoint, -1, self.home,
vrep.simx_opmode_oneshot)
aux1 = self.posicionIni[:]
for obj in self.objetos:
aux = random.choice(aux1)
vrep.simxSetObjectOrientation(self.clientID, obj, -1, self.oriObj3,
vrep.simx_opmode_oneshot)
vrep.simxSetObjectPosition(self.clientID, obj, -1,
(aux[0], aux[1], aux[2] + 0.06),
vrep.simx_opmode_oneshot)
aux1.remove(aux)
self.cont = 0
self.posEnMesa()
def moveObj(T, clientID, objHandle):
R = T[0:3, 0:3]
p = T[0:3, 3]
cy_thresh = 0
_FLOAT_EPS_4 = np.finfo(float).eps * 4.0
try:
cy_thresh = np.finfo(R.dtype).eps * 4
except ValueError:
cy_thresh = _FLOAT_EPS_4
r11, r12, r13, r21, r22, r23, r31, r32, r33 = R.flat
cy = math.sqrt(r33 * r33 + r23 * r23)
if cy > cy_thresh: # cos(y) not close to zero, standard form
z = math.atan2(-r12, r11) # atan2(cos(y)*sin(z), cos(y)*cos(z))
y = math.atan2(r13, cy) # atan2(sin(y), cy)
x = math.atan2(-r23, r33) # atan2(cos(y)*sin(x), cos(x)*cos(y))
else: # cos(y) (close to) zero, so x -> 0.0 (see above)
# so r21 -> sin(z), r22 -> cos(z) and
z = math.atan2(r21, r22)
y = math.atan2(r13, cy) # atan2(sin(y), cy)
x = 0.0
E = np.array([x, y, z])
vrep.simxSetObjectPosition(clientID, objHandle, -1, p,
vrep.simx_opmode_streaming)
vrep.simxSetObjectOrientation(clientID, objHandle, -1, E,
vrep.simx_opmode_oneshot)
def seat(
self, data, chair
): #Seats person at desired chair. In VREP, switches model from a standing bill to a sitting bill
data.bills.remove(self.billID)
data.billID = getBillID(data.bills)
self.type = "Seated"
self.location = chair.location
self.vrepOrientation = [0, 0, chair.vrepOrientation[2] - math.pi / 2]
if self.vrepID != None:
vrep.simxRemoveModel(clientID, self.vrepID,
vrep.simx_opmode_oneshot)
self.vrepPos = [
chair.vrepPos[0],
chair.vrepPos[1] - math.cos(chair.vrepOrientation[2]) * -0.05, 0
]
error, self.vrepID = vrep.simxLoadModel(
clientID, "/home/steelshot/vrep/models/people/IK Bill.ttm", 0,
vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(
clientID, self.vrepID, -1,
(self.vrepPos[0], self.vrepPos[1], self.vrepPos[2] - 0.4),
vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(clientID, self.vrepID, -1,
self.vrepOrientation,
vrep.simx_opmode_oneshot)
error, self.rightLegID = vrep.simxGetObjectHandle(
clientID, 'Bill_rightLegJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, rLeg = vrep.simxGetJointPosition(clientID, self.rightLegID,
vrep.simx_opmode_streaming)
error, self.leftLegID = vrep.simxGetObjectHandle(
clientID, 'Bill_leftLegJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.rightKneeID = vrep.simxGetObjectHandle(
clientID, 'Bill_rightKneeJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.leftKneeID = vrep.simxGetObjectHandle(
clientID, 'Bill_leftKneeJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.rightAnkleID = vrep.simxGetObjectHandle(
clientID, 'Bill_rightAnkleJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.leftAnkleID = vrep.simxGetObjectHandle(
clientID, 'Bill_leftAnkleJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.neckID = vrep.simxGetObjectHandle(
clientID, 'Bill_neck' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.rightArmID = vrep.simxGetObjectHandle(
clientID, 'Bill_rightShoulderJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.leftArmID = vrep.simxGetObjectHandle(
clientID, 'Bill_leftShoulderJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.rightElbowID = vrep.simxGetObjectHandle(
clientID, 'Bill_rightElbowJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.leftElbowID = vrep.simxGetObjectHandle(
clientID, 'Bill_leftElbowJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
def randomize_gripper_location(self):
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
_, sawyer_target_orientation = vrep.simxGetObjectOrientation(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
x_pos = np.random.uniform(low=1.028, high=1.242, size=1)[0]
y_pos = np.random.uniform(low=1.1, high=1.278, size=1)[0]
new_position = np.array([x_pos, y_pos, sawyer_target_position[2]])
orientation = np.random.uniform(low=0.01745329252, high=1.5533430343, size=1)[0]
new_orientation = np.array([sawyer_target_orientation[0], \
orientation, sawyer_target_orientation[2]])
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
move_direction = np.asarray([new_position[0] - sawyer_target_position[0], \
new_position[1] - sawyer_target_position[1], new_position[2] - \
sawyer_target_position[2]])
move_magnitude = np.linalg.norm(move_direction)
move_step = 0.01*move_direction/move_magnitude
num_move_steps = int(np.floor(move_magnitude/0.01))
remaining_magnitude = -num_move_steps * 0.01 + move_magnitude
remaining_distance = remaining_magnitude * move_direction/move_magnitude
for step_iter in range(num_move_steps): #selects action and executes action
vrep.simxSetObjectPosition(self.client_id, self.sawyer_target_handle, -1, \
(sawyer_target_position[0] + move_step[0], sawyer_target_position[1] + \
move_step[1], sawyer_target_position[2] + move_step[2]), vrep.simx_opmode_blocking)
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
vrep.simxSetObjectPosition(self.client_id, self.sawyer_target_handle, -1, \
(sawyer_target_position[0] + remaining_distance[0], sawyer_target_position[1] + \
remaining_distance[1], sawyer_target_position[2] + remaining_distance[2]), \
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
_, sawyer_orientation = vrep.simxGetObjectOrientation(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
rotation_step = 0.3 if (new_orientation[1] - sawyer_orientation[1] > 0) else -0.3
num_rotation_steps = int(np.floor((new_orientation[1] - \
sawyer_orientation[1]) / rotation_step))
for step_iter in range(num_rotation_steps):
vrep.simxSetObjectOrientation(self.client_id, self.sawyer_target_handle, \
-1, (sawyer_orientation[0], sawyer_orientation[1] + rotation_step, \
sawyer_orientation[2]), vrep.simx_opmode_blocking)
_, sawyer_orientation = vrep.simxGetObjectOrientation(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
vrep.simxSetObjectOrientation(self.client_id, self.sawyer_target_handle, \
-1, (sawyer_orientation[0], new_orientation[1], sawyer_orientation[2]), \
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def set_back(self):
vrep.simxSetObjectPosition(self.client_id, self.object_handle[0], -1, \
self.object_position, vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(self.client_id, self.object_handle[0], \
self.robot_handle, self.object_orientation, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def set_orientation(self, name, angles):
""" Sets the orientation of an object in VREP
Sets the orientation of an object using the provided Euler angles .
Parameters
----------
name : string
the name of the object of interest
angles : np.array
the [alpha, beta, gamma] Euler angles [radians]
for the object, specified in relative xyz axes.
"""
if self.misc_handles.get(name, None) is None:
# if we haven't retrieved the handle previously
# get the handle and set up streaming
_, self.misc_handles[name] = \
vrep.simxGetObjectHandle(self.clientID,
name,
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(
self.clientID,
self.misc_handles[name],
-1, # orientation relative to world
angles,
vrep.simx_opmode_blocking)
def set_initial_position(self):
self.open_hand()
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
move_direction = np.asarray([self.init_endpoint_pos[0] - sawyer_target_position[0], \
self.init_endpoint_pos[1] - sawyer_target_position[1], self.init_endpoint_pos[2] - \
sawyer_target_position[2]])
move_magnitude = np.linalg.norm(move_direction)
move_step = 0.01 * move_direction / move_magnitude
num_move_steps = int(np.floor(move_magnitude / 0.01))
for step_iter in range(num_move_steps):
vrep.simxSetObjectPosition(self.client_id, self.sawyer_target_handle, \
-1, (sawyer_target_position[0] + move_step[0], sawyer_target_position[1] + \
move_step[1], sawyer_target_position[2] + move_step[2]), vrep.simx_opmode_blocking)
_, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \
self.sawyer_target_handle, -1, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
vrep.simxSetObjectPosition(self.client_id, self.sawyer_target_handle, -1, \
self.init_endpoint_pos, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
vrep.simxSetObjectOrientation(self.client_id, self.sawyer_target_handle, -1, \
(self.init_endpoint_ori[0], self.init_endpoint_ori[1], self.init_endpoint_ori[2]), \
vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
for i in range(len(self.joint_handles)):
vrep.simxSetJointPosition(self.client_id, self.joint_handles[i], \
self.joint_positions[i], vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(self.client_id, self.joint_handles[i], -1, \
self.joint_orientation[i], vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(self.client_id)
vrep.simxGetPingTime(self.client_id)
def reset(self):
vrep.simxSetObjectOrientation(self.cid, self.mv_trg_handle, -1,
self.start_rot,
vrep.simx_opmode_blocking)
success = False
while not success:
super(ShelfStackEnv, self).reset()
start_pos = [0, 0, 0]
start_pos[0] = self.np_random.uniform(start_lower[0],
start_upper[0])
start_pos[1] = self.np_random.uniform(start_lower[1],
start_upper[1])
start_pos[2] = self.np_random.uniform(start_lower[2],
start_upper[2])
vrep.simxSetObjectPosition(self.cid, self.mv_trg_handle, -1,
start_pos, vrep.simx_opmode_blocking)
_, pose, _, _ = self.call_lua_function('solve_ik')
for handle, pos in zip(self.joint_handles, pose):
vrep.simxSetJointPosition(self.cid, handle, pos,
vrep.simx_opmode_blocking)
displacement = np.abs(
self.get_vector(self.mv_trg_handle, self.subject_handle))
orientation_diff = np.abs(self.get_mug_orientation())
success = np.all(orientation_diff <= max_rot) and np.all(
displacement <= 0.01)
vrep.simxSetObjectPosition(self.cid, self.anchor_handle,
self.mv_trg_handle, [0., 0., 0.],
vrep.simx_opmode_blocking)
return self._get_obs()
def joy_control_cb(self, data):
returnCode, self.target_sphere_handle = vrep.simxGetObjectHandle(
self.clientID, 'Target_sphere', vrep.simx_opmode_oneshot_wait)
position = []
if returnCode == 0:
returnCode, target_sphere_position = vrep.simxGetObjectPosition(
self.clientID, self.target_sphere_handle, -1,
vrep.simx_opmode_oneshot_wait)
returnCode, target_sphere_orentation = vrep.simxGetObjectOrientation(
self.clientID, self.target_sphere_handle, -1,
vrep.simx_opmode_oneshot_wait)
position[0] = target_sphere_position[
0] + 0.001 * data.buttons[3] - 0.001 * data.buttons[2]
position[1] = target_sphere_position[
1] + 0.001 * data.buttons[4] - 0.001 * data.buttons[1]
position[2] = target_sphere_position[
2] + 0.001 * data.buttons[6] - 0.001 * data.buttons[5]
orentation[0] = target_sphere_orentation[
0] + 0.01 * data.buttons[14] - 0.01 * data.buttons[15]
orentation[1] = target_sphere_orentation[
1] + 0.001 * data.buttons[4] - 0.001 * data.buttons[1]
orentation[2] = target_sphere_orentation[
2] + 0.01 * data.buttons[12] - 0.01 * data.buttons[13]
vrep.simxSetObjectPosition(self.clientID, self.target_sphere_handle,
-1, position, vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectOrientation(self.clientID, self.target_sphere_handle,
-1, orentation,
vrep.simx_opmode_oneshot_wait)
def render_human(self, chair, position, offset):
x, person = vrep.simxLoadModel(self.clientID,
self.models_path + "Sitting Bill.ttm",
0, vrep.simx_opmode_blocking)
x = vrep.simxSetObjectParent(self.clientID, person, chair, True,
vrep.simx_opmode_blocking)
offset_z = vrep.simxGetObjectFloatParameter(
self.clientID, person, vrep.sim_objfloatparam_modelbbox_max_z,
vrep.simx_opmode_blocking)[1]
vrep.simxSetObjectPosition(self.clientID, person, -1,
(position[0] + offset[0], position[1] +
offset[1], position[2] + offset[2]),
vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(self.clientID, person, -1,
(offset[3], offset[4], offset[5]),
vrep.simx_opmode_oneshot)
# vrep.simxSetModelProperty(self.clientID,person,vrep.sim_modelproperty_not_collidable,vrep.simx_opmode_oneshot)
# x, objs = vrep.simxGetObjects(self.clientID, vrep.sim_handle_all, vrep.simx_opmode_oneshot_wait)
# x, joint = vrep.simxGetObjectHandle(self.clientID, 'Bill_leftElbowJoint', vrep.simx_opmode_oneshot_wait)
# x, shoulder = vrep.simxGetObjectHandle(self.clientID, 'Bill_leftShoulderJoint', vrep.simx_opmode_oneshot_wait)
# rotationMatrix = (-1,0,0,0,0,-1,0,0,0,0,-1,0)
# x = vrep.simxSetSphericalJointMatrix(self.clientID, shoulder, rotationMatrix,vrep.simx_opmode_streaming)
#Moving Rotational Joint
# ang = -math.pi/2
# x = vrep.simxSetJointPosition(self.clientID, joint, ang, vrep.simx_opmode_oneshot)
# x, pos = vrep.simxGetJointPosition(self.clientID, joint, vrep.simx_opmode_streaming)
return person
def connectRobot(self):
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
print('Conectado a la remote API server with clientID: ', clientID)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait)
#pedir datos de objetos
errorCode1, handleJoint = vrep.simxGetObjectHandle(
clientID, 'm_Sphere', vrep.simx_opmode_oneshot_wait)
returnCode, home = vrep.simxGetObjectPosition(
clientID, handleJoint, -1, vrep.simx_opmode_blocking
) #deje estatico la primera posicion de target
vrep.simxSetObjectPosition(clientID, handleJoint, -1, home,
vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(clientID, handleJoint, -1,
(0, math.pi, 0),
vrep.simx_opmode_oneshot)
#inicio camara kinect
errorCode, rgb = vrep.simxGetObjectHandle(
clientID, 'kinect_rgb', vrep.simx_opmode_oneshot_wait)
res, resolution, imegenRgb = vrep.simxGetVisionSensorImage(
clientID, rgb, 0, vrep.simx_opmode_streaming)
else:
print('Error de conexion a remote API server')
return clientID, home
def set_angle(self, name, phi):
try:
vrep.simxSetObjectOrientation(self.clientID,
self.models[name].handle, -1,
[0, 0, math.radians(phi)],
vrep.simx_opmode_oneshot_wait)
except KeyError:
print("Could not set Poistion for ", name)
def set_pos_ori_robot(self, x=0.0, y=0.0, gamma=0.0):
""" set both the position (x, y) and the orientation (gamma) of the robot """
vrep.simxSetObjectPosition(self.clientID, self.robot_handle, -1,
[x, y, +1.3879e-01],
vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(self.clientID, self.robot_handle, -1,
[0.0000e+00, 0.0000e+00, gamma],
vrep.simx_opmode_oneshot)
return x, y, gamma
def lookAnimate(self, object): #Same as for agent
ang = -math.atan2((self.location[1] - object.location[1]),
(self.location[0] - object.location[0]))
self.vrepOrientation = [
self.vrepOrientation[0], self.vrepOrientation[1], ang
]
vrep.simxSetObjectOrientation(clientID, self.vrepID, -1,
self.vrepOrientation,
vrep.simx_opmode_oneshot)
def set_position(self, position):
"""Set the position (x,y,theta) of the robot
Args:
position (list): the position [x,y,theta]
"""
vrep.simxSetObjectPosition(self.client_id, self.pioneer, -1, [position[0], position[1], 0.13879], vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectOrientation(self.client_id, self.pioneer, -1, [0, 0, to_deg(position[2])], vrep.simx_opmode_oneshot_wait)
def set_object_pose(clientID, pose, obj_name):
predicted_position = pose[0:3, 3]
predicted_orient = rot_matrix_to_euler_angles(pose[0:3, 0:3])
vrep.simxSetObjectPosition(clientID, get_handle(clientID, obj_name),
get_handle(clientID, 'UR3_link1_visible'),
predicted_position, vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(clientID, get_handle(clientID, obj_name),
get_handle(clientID, 'UR3_link1_visible'),
predicted_orient, vrep.simx_opmode_oneshot)
def set_position(self, position):
"""Set the position (x,y,theta) of the robot
Args:
position (list): the position [x,y,theta]
"""
vrep.simxSetObjectPosition(self.client_id, self.pioneer, -1, [position[0], position[1], 0.13879], vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectOrientation(self.client_id, self.pioneer, -1, [0, 0, to_deg(position[2])], vrep.simx_opmode_oneshot_wait)
def set_object_orientation(self, handle, orientation):
"""
Set the angular orientation of an object in vrep
"""
vrep.simxSetObjectOrientation(self.client_id,
handle,
-1,
orientation,
vrep.simx_opmode_oneshot)
def single_object_evaluation():
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
test = 4824
if clientID != -1:
while True:
# Initialize environment
panda = Panda(clientID)
obj_pos, obj_ori, handles = panda.init_env()
pointcloud = panda.get_cloud()
grasp_poses, points = new_grasp_pose_generation(pointcloud, 10)
print('Found %d poses' % len(grasp_poses))
res, tip_hdl = vrep.simxGetObjectHandle(clientID, 'tip',
vrep.simx_opmode_blocking)
res, tip_xyz = vrep.simxGetObjectPosition(
clientID, tip_hdl, -1, vrep.simx_opmode_oneshot_wait)
print('tip is at: ', tip_xyz)
# ----------------------------------------------------------------------------------------------------------
for i in range(len(grasp_poses)):
panda.grasp(grasp_poses[i], points[i])
filename = '/home/lou00015/dataset/collision_nn/pose_' + str(
test) + '.npy'
tbsaved = grasp_poses[i]
tbsaved.append(points[i])
np.save(filename, tbsaved)
# ----------------------------------------------------------------------------------------------------------
res, current_pos = vrep.simxGetObjectPosition(
clientID, tip_hdl, -1, vrep.simx_opmode_oneshot_wait)
print('current tip at: ', current_pos)
if current_pos[0] != tip_xyz[0]:
label = 1
else:
label = 0
copyfile(
'/home/lou00015/cnn3d/scripts/data.pcd',
'/home/lou00015/dataset/collision_nn/test' + str(test) +
'.pcd')
f = open('/home/lou00015/dataset/collision_nn/label.txt', "a+")
f.write(str(label))
f.close()
test = test + 1
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
time.sleep(3)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, handles[0], -1,
obj_pos[0],
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(clientID, handles[0], -1,
obj_ori[0],
vrep.simx_opmode_blocking)
else:
print(
'Failed to connect to simulation (V-REP remote API server). Exiting.'
)
exit()
def createVrep(self, pos, orientation):
error, self.vrepID = vrep.simxLoadModel(
clientID,
"/home/steelshot/vrep/models/furniture/chairs/dining chair.ttm", 0,
vrep.simx_opmode_blocking)
self.vrepPos = pos
self.vrepOrientation = orientation
vrep.simxSetObjectPosition(clientID, self.vrepID, -1,
(pos[0], pos[1], pos[2]),
vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(clientID, self.vrepID, -1, orientation,
vrep.simx_opmode_oneshot)
def step(self, v):
self.action(v)
obs = self.observe()
rew = self.reward()
end = self.done
vrep.simxSetObjectOrientation(self.clientID, self.robot, -1,
[0, 0, math.pi],
vrep.simx_opmode_oneshot)
return obs, rew, end
def set_target_position(pos):
if len(pos) < 4:
pos.append(default_height)
now_pos = get_drone_position()
print('move target from ', round(now_pos[0], 2), ',', round(now_pos[1], 2), ',', round(now_pos[2], 2), ' to ',
pos[1], ',', pos[2], ',', pos[3], "rotate(deg) :", pos[0])
vrep.simxSetObjectOrientation(clientID, target, base, [0, 0, math.radians(pos[0])], vrep.simx_opmode_oneshot)
vrep.simxSetObjectPosition(clientID=clientID,
objectHandle=target,
relativeToObjectHandle=-1,
position=pos[1:],
operationMode=vrep.simx_opmode_oneshot)
def multiple_objects_evaluation_no_pushing():
grasping_model = load_model('trained_models/grasping.h5')
# for layer in grasping_model.layers:
# layer.name = layer.name + '_grasping'
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID != -1:
while True:
# Initialize environment
panda = Panda(clientID)
obj_pos, obj_ori, handles = panda.init_env()
handles_left = len(handles)
while handles_left > 0:
pointcloud = panda.get_cloud()
grasp_poses, points = grasp_pose_generation('data.pcd')
grasp_sc = grasp_scores(grasping_model, grasp_poses, points,
pointcloud)
print('Grasping scores:')
for scores in grasp_sc:
print(scores)
# ----------------------------------------------------------------------------------------------------------
if len(grasp_sc) != 0:
max_index = np.argmax(grasp_sc)
print('Highest grasping score index: ', max_index)
panda.grasp(grasp_poses[max_index], points[max_index])
# ----------------------------------------------------------------------------------------------------------
print('Checking results')
for j in range(3):
res, current_pos = vrep.simxGetObjectPosition(
clientID, handles[j], -1,
vrep.simx_opmode_oneshot_wait)
print(current_pos)
if current_pos[2] > obj_pos[j][2] + 0.03:
vrep.simxSetObjectPosition(
clientID, handles[j], -1, [2, 2, 0.5],
vrep.simx_opmode_oneshot_wait)
obj_pos[j] = [2, 2, 0.5]
handles_left = handles_left - 1
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
time.sleep(3)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
for k in range(3):
vrep.simxSetObjectPosition(clientID, handles[k], -1,
obj_pos[k],
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(clientID, handles[k], -1,
obj_ori[k],
vrep.simx_opmode_blocking)
else:
print(
'Failed to connect to simulation (V-REP remote API server). Exiting.'
)
exit()
def createVrep(self, pos, orientation): #Create waiter model
error, self.vrepID = vrep.simxLoadModel(
clientID,
"/home/steelshot/vrep/models/people/Walking Bill Chef Bot.ttm", 0,
vrep.simx_opmode_blocking)
self.type = "Walking"
error, self.moveBoxID = vrep.simxGetObjectHandle(
clientID, "Bill_goalDummy" + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
self.vrepPos = pos
self.vrepOrientation = orientation
vrep.simxSetObjectPosition(clientID, self.vrepID, -1,
(pos[0], pos[1], pos[2]),
vrep.simx_opmode_oneshot)
vrep.simxSetObjectPosition(clientID, self.moveBoxID, -1,
(pos[0], pos[1], pos[2]),
vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(clientID, self.vrepID, -1, orientation,
vrep.simx_opmode_oneshot)
error, self.rightLegID = vrep.simxGetObjectHandle(
clientID, 'Bill_rightLegJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.leftLegID = vrep.simxGetObjectHandle(
clientID, 'Bill_leftLegJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.rightKneeID = vrep.simxGetObjectHandle(
clientID, 'Bill_rightKneeJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.leftKneeID = vrep.simxGetObjectHandle(
clientID, 'Bill_leftKneeJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.rightAnkleID = vrep.simxGetObjectHandle(
clientID, 'Bill_rightAnkleJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.leftAnkleID = vrep.simxGetObjectHandle(
clientID, 'Bill_leftAnkleJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.neckID = vrep.simxGetObjectHandle(
clientID, 'Bill_neck' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.rightArmID = vrep.simxGetObjectHandle(
clientID, 'Bill_rightShoulderJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.leftArmID = vrep.simxGetObjectHandle(
clientID, 'Bill_leftShoulderJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.rightElbowID = vrep.simxGetObjectHandle(
clientID, 'Bill_rightElbowJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
error, self.leftElbowID = vrep.simxGetObjectHandle(
clientID, 'Bill_leftElbowJoint' + getBillString(self.billID),
vrep.simx_opmode_oneshot_wait)
def pushing_data():
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
test = 1336
if clientID != -1:
while True:
# Initialize environment
panda = Panda(clientID)
obj_pos, obj_ori, handles = panda.init_env()
pointcloud = panda.get_cloud()
nb_clutters = segmentation('data.pcd')
print("Found %d clouds before pushing" % nb_clutters)
push_poses = push_pose_generation(pointcloud, 30)
# ----------------------------------------------------------------------------------------------------------
for p_index in range(len(push_poses)):
panda.push(push_poses[p_index])
cloud_new = panda.get_cloud()
nb_clutters_new = segmentation('data.pcd')
print("Found %d clouds after pushing" % nb_clutters_new)
if nb_clutters_new > nb_clutters:
result = 1
else:
result = 0
r_cloud = push_transform(push_poses[p_index], pointcloud)
np.savetxt('forthehorde.pcd',
r_cloud,
fmt='%1.9f',
delimiter=' ')
insertHeader('forthehorde.pcd')
copyfile(
'/home/lou00015/cnn3d/scripts/forthehorde.pcd',
'/home/lou00015/dataset/push_UR5/test' + str(test) +
'.pcd')
f = open('/home/lou00015/dataset/push_UR5/label.txt', "a+")
f.write(str(result))
f.close()
test = test + 1
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
time.sleep(3)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
for j in range(8):
vrep.simxSetObjectPosition(clientID, handles[j], -1,
obj_pos[j],
vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectOrientation(
clientID, handles[j], -1, obj_ori[j],
vrep.simx_opmode_oneshot_wait)
else:
print(
'Failed to connect to simulation (V-REP remote API server). Exiting.'
)
exit()
def reset(self):
super(ReachOverWallEnv, self).reset()
self.target_pos[0] = self.np_random.uniform(cube_lower[0], cube_upper[0])
self.target_pos[1] = self.np_random.uniform(cube_lower[1], cube_upper[1])
vrep.simxSetObjectPosition(self.cid, self.sphere_handle, -1, self.target_pos,
vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(self.cid, self.wall_handle, -1, self.wall_pos,
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(self.cid, self.wall_handle, -1, self.init_wall_rot,
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(self.cid, self.mv_trg_handle, -1, [0., 0., 0.],
vrep.simx_opmode_blocking)
return self._get_obs()
def more_data():
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
test = 0
if clientID != -1:
while True:
# Initialize environment
panda = Panda(clientID)
obj_pos, obj_ori, handles = panda.init_env()
pointcloud = panda.get_cloud()
grasp_poses, points = grasp_pose_generation('data.pcd')
# ----------------------------------------------------------------------------------------------------------
for g_index in range(len(grasp_poses)):
print(grasp_poses[g_index], points[g_index])
panda.grasp(grasp_poses[g_index], points[g_index])
res, current_pos = vrep.simxGetObjectPosition(
clientID, handles[0], -1, vrep.simx_opmode_oneshot_wait)
print(current_pos)
if current_pos[2] > obj_pos[0][2] + 0.03:
result = 1
else:
result = 0
r_cloud = rotate_cloud(points[g_index], grasp_poses[g_index],
pointcloud)
np.savetxt('forthehorde.pcd',
r_cloud,
fmt='%1.9f',
delimiter=' ')
insertHeader('forthehorde.pcd')
copyfile(
'/home/lou00015/cnn3d/scripts/forthehorde.pcd',
'/home/lou00015/dataset/data_UR5/test' + str(test) +
'.pcd')
f = open('/home/lou00015/dataset/data_UR5/label.txt', "a+")
f.write(str(result))
f.close()
test = test + 1
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
time.sleep(3)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, handles[0], -1,
obj_pos[0],
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(clientID, handles[0], -1,
obj_ori[0],
vrep.simx_opmode_blocking)
else:
print(
'Failed to connect to simulation (V-REP remote API server). Exiting.'
)
exit()
def robotCode(self):
""" OUR ROBOT CODE GOES HERE """
# Shooting Test
vrep.simxSetObjectPosition(
self.clientID, self.ball,-1,[0, -0.35], vrep.simx_opmode_streaming)
target = self.aim()
vrep.simxSetObjectPosition(
self.clientID, self.player2, -1, target, vrep.simx_opmode_streaming)
position = []
position = self.kickingPose(target)
kickingPosition = [position[0], position[1], z]
vrep.simxSetObjectPosition(
self.clientID, self.bot, -1, kickingPosition, vrep.simx_opmode_streaming)
vrep.simxSetObjectOrientation(
self.clientID, self.bot, -1, [self.eulerAngles[0], self.eulerAngles[1], position[2]], vrep.simx_opmode_streaming)
time.sleep(2)
def set_target(self,x,y,z,rot):
vrep.simxSetObjectPosition(self.clientID,self.target,-1,(x,y,z), vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(self.clientID,self.target,-1,(0,0,rot), vrep.simx_opmode_oneshot)
self.oldtarget = (x,y,z,rot)
def orientation(cid, handle, val):
err = vrep.simxSetObjectOrientation(cid, handle, -1, val, vrep_mode)
def setObjectOrientation(self, obj, orientation):
return vrep.simxSetObjectOrientation(self.clientID, obj, -1, orientation, vrep.simx_opmode_buffer)
alpha = [1/4,1/4,1/(math.pi)]
print ('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
client_id=vrep.simxStart(ip,port,True,True,5000,5) # Connect to V-REP
if client_id!=-1:
print ('Connected to remote API server on %s:%s' % (ip, port))
res = vrep.simxLoadScene(client_id, scene, 1, vrep.simx_opmode_oneshot_wait)
res, pioneer = vrep.simxGetObjectHandle(client_id, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
res, left_motor = vrep.simxGetObjectHandle(client_id, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
res, right_motor = vrep.simxGetObjectHandle(client_id, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)
# set initial position, -1 is absolute
vrep.simxSetObjectPosition(client_id, pioneer, -1, [position_init[0], position_init[1], 0.13879], vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectOrientation(client_id, pioneer, -1, [0, 0, to_deg(position_init[2])], vrep.simx_opmode_oneshot_wait)
network = NN(ni,nj,nk)
vrep.simxStartSimulation(client_id, vrep.simx_opmode_oneshot_wait)
position = position_init # [x,y,theta]
network_input = [0, 0, 0]
for i in range(ni):
network_input[i] = position[i]-target[i]
prev_error = 100000
#while(distance(position, target)>0.001 or position[2]-target[2]>0.1):
while(True):
#current_time = time.time()
b2=b1[next+1]
c2=c1[next+1]
else:
a2=a
b2=b
c2=c
adiff=a2-a
bdiff=b2-b
cdiff=c2-c
x=0.4+0.4*a
y=0.2+0.4*b
z=0.3+0.4*c
returnCode,newObjectHandles=vrep.simxCopyPasteObjects(clientID,objectHandles,vrep.simx_opmode_oneshot_wait)
Arro=newObjectHandles[0]
vrep.simxSetObjectPosition (clientID,Arro,-1,(0,0,0),vrep.simx_opmode_oneshot)
returnCode=vrep.simxSetObjectOrientation(clientID,Arro,-1,[angle_calculationx([0,bdiff,cdiff],[0,0,-1]),-angle_calculationy([0,bdiff,cdiff],[adiff,bdiff,cdiff]),0],vrep.simx_opmode_oneshot)
vrep.simxSetObjectPosition (clientID,Arro,-1,(x,y,z),vrep.simx_opmode_oneshot)
#Quadratic
errorCode,Arrow=vrep.simxGetObjectHandle(clientID,'Arrow2',vrep.simx_opmode_oneshot_wait)
objectHandles=np.array([Arrow])
#put arrows on every point the trajectory pointing in the direction of the next point
for next in range(100):
a1=quadratic[0]
b1=quadratic[1]
c1=quadratic[2]
a=a1[next]
b=b1[next]
c=c1[next]
if next<99:
a2=a1[next+1]
b2=b1[next+1]
