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_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]