def __init__(self, port=19997): self.sim_client = utils.connectToSimulation('127.0.0.1', port) self.rdd = RDD(self.sim_client, open_force=20, close_force=20) self.ur5 = UR5(self.sim_client, self.rdd) self.nA = 5 self.cube = None self.cube_start_position = [-0.35, 0.7, 0.03] self.cube_size = [0.1, 0.1, 0.1] self.open_position = 0.3
def __init__(self, port=19997): self.sim_client = utils.connectToSimulation('127.0.0.1', port) # Create UR5 and restart simulator self.rdd = RDD(self.sim_client, open_force=10) self.ur5 = UR5(self.sim_client, self.rdd) self.nA = 2 self.cube = None self.cube_start_position = [-0.2, 0.9, 0.05] self.cube_size = [0.1, 0.2, 0.04]
def __init__(self, port=19997, memory_size=60): self.sim_client = utils.connectToSimulation('127.0.0.1', port) # Create UR5 and restart simulator self.rdd = RDD(self.sim_client) self.ur5 = UR5(self.sim_client, self.rdd) self.nA = 2 self.cube = None self.cube_start_position = [-0.2, 0.85, 0.025] self.cube_size = [0.1, 0.2, 0.04] self.open_position = 0.3
def main(): rospy.init_node('simple_test') # Attempt to connect to simulator sim_client = utils.connectToSimulation('127.0.0.1', 19997) vrep.simxStopSimulation(sim_client, VREP_BLOCKING) time.sleep(1) vrep.simxStartSimulation(sim_client, VREP_BLOCKING) time.sleep(1) rdd = RDD(sim_client) ur5 = UR5(sim_client, rdd) # Generate a cube position = [-0.2, 0.9, 0.05] orientation = [0, 0, 0] # orientation = [0, 0, 0] size = [0.1, 0.2, 0.05] mass = 1 color = [255, 0, 0] cube = utils.generateShape(sim_client, 'cube', 0, size, position, orientation, mass, color) time.sleep(1) pose = ur5.getEndEffectorPose() target_pose = pose.copy() # rdd.openFinger(RDD.NARROW) narrow_finger_joint = rdd.finger_joint_narrow.joint def print_joint_pos(joint): while True: # print utils.getJointPosition(sim_client, joint) res, cube_pose = utils.getObjectPosition(sim_client, cube) while any(np.isnan(cube_pose)): res, cube_pose = utils.getObjectPosition(sim_client, cube) print cube_pose thread.start_new_thread(print_joint_pos, (narrow_finger_joint, )) target_pose[1, 3] += 0.3 rdd.openFinger(RDD.NARROW) ur5.moveTo(target_pose) ur5.moveTo(pose)
def __init__(self, port=19997, memory_size=60): rospy.init_node('env', anonymous=True) self.sim_client = utils.connectToSimulation('127.0.0.1', port) # Create UR5 and restart simulator self.rdd = RDD(self.sim_client) self.ur5 = UR5(self.sim_client, self.rdd) self.nA = 4 self.cube = None self.cube_start_position = [-0.2, 0.85, 0.025] self.cube_size = [0.1, 0.2, 0.04] self.open_position = 0.3 self.narrow_position = None self.wide_position = None self.rdd_position = [0 for _ in range(2 * memory_size)] self.rdd_force = [0 for _ in range(2 * memory_size)] self.narrow_p = [0 for _ in range(memory_size)] self.narrow_t = [0 for _ in range(memory_size)] self.rdd_sub = rospy.Subscriber('sim/rdd_joints', Float32MultiArray, self.rddJointsCallback, queue_size=1) self.tip_position = None self.tip_orientation = None self.tip_pos_sub = rospy.Subscriber('sim/ur5_tip_pose', Float32MultiArray, self.tipPosCallback, queue_size=1) self.target_position = None self.target_orientation = None self.target_pos_sub = rospy.Subscriber('sim/ur5_target_pose', Float32MultiArray, self.targetPosCallback, queue_size=1) self.cube_position = None self.cube_orientation = None self.cube_pos_sub = rospy.Subscriber('sim/cube_pose', Float32MultiArray, self.cubePosCallback, queue_size=1)
def __init__(self, port=19997): np.random.seed(port) self.sim_client = utils.connectToSimulation('127.0.0.1', port) # Create UR5 and restart simulator self.rdd = RDD(self.sim_client) self.ur5 = UR5(self.sim_client, self.rdd) self.sensor = VisionSensor(self.sim_client, 'Vision_sensor', None, None, True, False) self.nA = 4 self.observation_space = np.zeros(4) self.cube = None self.cube_start_position = [-0.2, 0.85, 0.025] self.cube_size = [0.1, 0.2, 0.04] self.open_position = 0.3
def __init__(self, port=19997): rospy.init_node('env', anonymous=True) self.sim_client = utils.connectToSimulation('127.0.0.1', port) # Create UR5 and restart simulator self.rdd = RDD(self.sim_client, open_force=10, close_force=20) self.ur5 = UR5(self.sim_client, self.rdd) self.nA = 3 self.cube = None self.cube_start_position = [-0.2, 0.9, 0.05] self.cube_size = [0.1, 0.2, 0.04] self.open_position = 0.3 self.state = [0 for _ in range(4 * 60)] self.stateSub = rospy.Subscriber('sim/state', Float32MultiArray, self.stateCallback)
def __init__(self, port=19997): np.random.seed(port) self.sim_client = utils.connectToSimulation('127.0.0.1', port) # Create UR5 and restart simulator self.rdd = RDD(self.sim_client) self.ur5 = UR5(self.sim_client, self.rdd) self.sensor = VisionSensor(self.sim_client, 'Vision_sensor_top', None, None, True, False) self.nA = 4 self.observation_space = (np.zeros((12, 64, 64)), np.zeros((4, 20))) self.cube = None self.cube_start_position = [-0.2, 0.85, 0.025] self.cube_size = [0.1, 0.2, 0.04] self.open_position = 0.3 self.img_his = [np.zeros((3, 64, 64)) for _ in range(4)] self.theta_his = [np.zeros((1, 20)) for _ in range(4)]