Ejemplo n.º 1
0
    def startSimulation(self):
        if self.clientId != -1:
            #+++++++++++++++++++++++++++++++++++++++++++++
            step = 0.005
            vrep.simxSetFloatingParameter(
                self.clientId, vrep.sim_floatparam_simulation_time_step, step,
                vrep.simx_opmode_oneshot)
            vrep.simxSynchronous(self.clientId, True)
            vrep.simxStartSimulation(self.clientId, vrep.simx_opmode_oneshot)
            #init the controller
            planeController = PlaneCotroller(self.clientId)
            planeController.take_off()
            self.pdController = controller.PID(cid=self.clientId,
                                               ori_mode=True)

            #create a thread to controll the move of quadcopter
            _thread.start_new_thread(self.pdThread, ())
            print("thread created")
            #to be stable
            planeController.loose_jacohand()
            # planeController.move_to(planeController.get_object_pos(planeController.copter),True)
            # planeController.plane_pos = planeController.get_object_pos(planeController.copter)
            # time.sleep(10)
            self.run_simulation(planeController)
        else:
            print('Failed connecting to remote API server')
        print('Simulation ended')
        vrep.simxStopSimulation(self.clientId, vrep.simx_opmode_oneshot)
Ejemplo n.º 2
0
	def start_sim(self):
		# vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot_wait)
		vrep.simxSynchronous(self.clientID, True)
		dt = .001
		vrep.simxSetFloatingParameter(self.clientID,vrep.sim_floatparam_simulation_time_step,dt,vrep.simx_opmode_oneshot)
		vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot);
		return
Ejemplo n.º 3
0
    def _connect_to_vrep(self):

        # Initialise Communication Line #
        portNb = 19997
        vrep.simxFinish(-1)
        self.clientID = vrep.simxStart('127.0.0.1', portNb, True, True, 2000,
                                       5)

        # Connect to vrep
        if self.clientID != -1:
            print('connection made')
            _, self.jointHandles[0] = vrep.simxGetObjectHandle(
                self.clientID, 'Shoulder', vrep.simx_opmode_blocking)
            _, self.tipHandle = vrep.simxGetObjectHandle(
                self.clientID, 'tip', vrep.simx_opmode_blocking)
            vrep.simxSynchronous(
                self.clientID,
                True)  # Enable the synchronous mode (Blocking function call)
        else:
            print('no connection')

        # set time-step
        vrep.simxSetFloatingParameter(self.clientID,
                                      vrep.sim_floatparam_simulation_time_step,
                                      self._hyperparams['dt'],
                                      vrep.simx_opmode_blocking)
def startSim():
    global clientID, arm_joint, mobile_joint, joint_handles, gripper_handles, throttle_handles, body_handle, sphere_handle

    vrep.simxSetFloatingParameter(
        clientID,
        vrep.sim_floatparam_simulation_time_step,
        deltaTime,  # specify a simulation time step
        vrep.simx_opmode_oneshot)

    # --------------------- Start the simulation

    # start our simulation in lockstep with our code
    vrep.simxSynchronous(clientID, True)
    vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)

    joint_handles = [
        vrep.simxGetObjectHandle(clientID, name, vrep.simx_opmode_blocking)[1]
        for name in arm_joint
    ]

    gripper_handles = [
        vrep.simxGetObjectHandle(clientID, name, vrep.simx_opmode_blocking)[1]
        for name in gripper_joint
    ]

    throttle_handles = [
        vrep.simxGetObjectHandle(clientID, name, vrep.simx_opmode_blocking)[1]
        for name in mobile_joint
    ]

    body_handle = vrep.simxGetObjectHandle(clientID, body_name,
                                           vrep.simx_opmode_blocking)[1]

    sphere_handle = vrep.simxGetObjectHandle(clientID, spherical_target,
                                             vrep.simx_opmode_blocking)[1]
Ejemplo n.º 5
0
 def startSim(self, clientID, screen=True):
     vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_dynamic_engine,bulletEngine ,vrep.simx_opmode_oneshot_wait)
     vrep.simxSetFloatingParameter(clientID,vrep.sim_floatparam_simulation_time_step, simulatioTimeStepDT, vrep.simx_opmode_oneshot_wait)
     error=vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot)
     if not screen:
         vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_visible_layers,2,vrep.simx_opmode_oneshot_wait)
         #vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_display_enabled,0,vrep.simx_opmode_oneshot_wait)
     return error
Ejemplo n.º 6
0
    def connect(self):
        """ Connect to the current scene open in VREP

        Finds the VREP references to the joints of the robot.
        Sets the time step for simulation and put into lock-step mode.

        NOTE: the joints and links must follow the naming convention of
        'joint#' and 'link#', starting from 'joint0' and 'link0'

        NOTE: The dt in the VREP physics engine must also be specified
        to be less than the dt used here.
        """

        # close any open connections
        vrep.simxFinish(-1)
        # Connect to the V-REP continuous server
        self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 500, 5)

        if self.clientID == -1:
            raise Exception('Failed connecting to remote API server')

        vrep.simxSynchronous(self.clientID, True)

        # get the handles for each joint and set up streaming
        self.joint_handles = [vrep.simxGetObjectHandle(self.clientID,
                                                       name,
                                                       vrep.simx_opmode_blocking)[1] for name in
                              self.robot_config.JOINT_NAMES]

        # get handle for target and set up streaming
        _, self.misc_handles['target'] = \
            vrep.simxGetObjectHandle(self.clientID,
                                     'target',
                                     vrep.simx_opmode_blocking)
        # get handle for hand
        _, self.hand_handle = \
            vrep.simxGetObjectHandle(self.clientID,
                                     'hand',
                                     vrep.simx_opmode_blocking)

        vrep.simxSetFloatingParameter(
            self.clientID,
            vrep.sim_floatparam_simulation_time_step,
            self.dt,  # specify a simulation time step
            vrep.simx_opmode_oneshot)

        vrep.simxSetBooleanParameter(
            self.clientID,
            vrep.sim_boolparam_display_enabled,
            True,
            vrep.simx_opmode_oneshot)

        # start our simulation in lockstep with our code
        vrep.simxStartSimulation(self.clientID,
                                 vrep.simx_opmode_blocking)

        print('Connected to VREP remote API server')
Ejemplo n.º 7
0
 def start_sim(self):
     # Set Simulator
     vrep.simxSynchronous(self.clientID, True)
     dt = .05
     vrep.simxSetFloatingParameter(self.clientID,
                                   vrep.sim_floatparam_simulation_time_step,
                                   dt,  # specify a simulation time step
                                   vrep.simx_opmode_oneshot)
     vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
     return
Ejemplo n.º 8
0
 def set_float_parameter(self, parameter_id, value):
     """
     Sets a float parameter
     """
     return vrep.simxSetFloatingParameter(self.client_id, parameter_id,
                                          value,
                                          vrep.simx_opmode_blocking)[0]
    def start_simulation(self):
        if self.robots_connected == 0:
            print("no robots connected, simulation not started")
        else:
            # Set up streaming
            print("{} robot(s) connected: {}".format(self.robots_connected,
                                                     self.robot_names))
            vrep.simxSetFloatingParameter(
                self.clientID,
                vrep.sim_floatparam_simulation_time_step,
                self.dt,  # specify a simulation time stept
                vrep.simx_opmode_oneshot)

            # Start the simulation
            #vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_blocking) #to increase loop speed mode is changed.
            vrep.simxStartSimulation(self.clientID,
                                     vrep.simx_opmode_oneshot_wait)
Ejemplo n.º 10
0
 def __init__(self):
     # 建立通信
     vrep.simxFinish(-1)
     # 每隔0.2s检测一次,直到连接上V-rep
     while True:
         self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True,
                                        5000, 5)
         if self.clientID != -1:
             break
         else:
             time.sleep(0.1)
             print("Failed connecting to remote API server!")
     print("Connection success!")
     #设置机械臂仿真步长,为保持API端与VREP端相同步长
     vrep.simxSetFloatingParameter(self.clientID,
                                   vrep.sim_floatparam_simulation_time_step,
                                   self.dt, vrep.simx_opmode_oneshot)
     # 打开同步模式
     vrep.simxSynchronous(self.clientID, True)
     vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)
     # 获取句柄joint
     self.robot1_jointHandle = np.zeros((self.jointNum, ),
                                        dtype=np.int)  # joint 句柄
     for i in range(self.jointNum):
         _, returnHandle = vrep.simxGetObjectHandle(
             self.clientID, self.jointName + str(i + 1),
             vrep.simx_opmode_blocking)
         self.robot1_jointHandle[i] = returnHandle
     # 获取末端frame
     _, self.end_handle = vrep.simxGetObjectHandle(
         self.clientID, 'end', vrep.simx_opmode_blocking)
     _, self.goal_handle = vrep.simxGetObjectHandle(
         self.clientID, 'goal_1', vrep.simx_opmode_blocking)
     # 障碍最短距离handle
     _, self.dist_handle = vrep.simxGetDistanceHandle(
         self.clientID, 'dis_robot1', vrep.simx_opmode_blocking)
     _, self.end_dis_handle = vrep.simxGetDistanceHandle(
         self.clientID, 'robot1_goal', vrep.simx_opmode_blocking)
     # 获取link句柄
     self.link_handle = np.zeros((self.jointNum, ), dtype=np.int)  #link 句柄
     for i in range(self.jointNum):
         _, returnHandle = vrep.simxGetObjectHandle(
             self.clientID, self.linkName + str(i + 1),
             vrep.simx_opmode_blocking)
         self.link_handle[i] = returnHandle
     print('Handles available!')
Ejemplo n.º 11
0
def link_vrep(link=False):  # 连接vrep

    print('link vrep....')
    # 关闭潜在的链接
    vrep.simxFinish(-1)

    clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000,
                              5)  # Connect to V-REP
    if clientID != -1:
        link = True
        print('Success Connected to remote API server')
    vrep.simxSetFloatingParameter(clientID,
                                  vrep.sim_floatparam_simulation_time_step,
                                  tstep, vrep.simx_opmode_oneshot)
    vrep.simxSynchronous(clientID, True)  ###打开同步模式
    vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
    return clientID, link
Ejemplo n.º 12
0
 def connect(self):  #连接v-rep
     print('Program started')  # 关闭潜在的连接
     vrep.simxFinish(-1)  # 每隔0.2s检测一次,直到连接上V-rep
     while True:
         self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True,
                                        5000, 5)
         if self.clientID > -1:
             break
         else:
             time.sleep(0.2)
             print("Failed connecting to remote API server!")
     print("Connection success!")
     vrep.simxSetFloatingParameter(
         self.clientID, vrep.sim_floatparam_simulation_time_step,
         self.tstep, vrep.simx_opmode_oneshot)  # 保持API端与V-rep端相同步长
     vrep.simxSynchronous(self.clientID, True)  # 然后打开同步模式
     vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)
Ejemplo n.º 13
0
    def __init__(self, ip, port, time_step, actuator_names, robot_names,
                 object_names):
        # Connect to the V-REP continuous server
        self.clientID = vrep.simxStart(ip, port, True, True, -500000, 5)
        if self.clientID != -1:  # if we connected successfully
            print('Connected to remote API server')
        else:
            raise Exception()
        # -------Setup the simulation
        vrep.simxSynchronous(self.clientID, True)  #if we need to be syncronous
        # move simulation ahead one time step
        vrep.simxSynchronousTrigger(self.clientID)

        vrep.simxSetFloatingParameter(
            self.clientID,
            vrep.sim_floatparam_simulation_time_step,
            time_step,  # specify a simulation time step
            vrep.simx_opmode_oneshot)

        self.time_step = time_step
        self.act_names = actuator_names
        # get the handles for each motor and set up streaming
        self.act_handles = {}
        for name in actuator_names:
            _, obj_handle = vrep.simxGetObjectHandle(self.clientID, name,
                                                     vrep.simx_opmode_blocking)
            if _ != 0: raise Exception()
            self.act_handles.update({name: obj_handle})

        # get handle for target and set up streaming
        self.obj_handles = {}
        for name in object_names:
            _, obj_handle = vrep.simxGetObjectHandle(self.clientID, name,
                                                     vrep.simx_opmode_blocking)
            if _ != 0: raise Exception()
            self.obj_handles.update({name: obj_handle})

        # get robot handle
        self.ddr_handles = {}
        for name in robot_names:
            _, obj_handle = vrep.simxGetObjectHandle(self.clientID, name,
                                                     vrep.simx_opmode_blocking)
            if _ != 0: raise Exception()
            self.ddr_handles.update({name: obj_handle})
Ejemplo n.º 14
0
    def __init__(self):
        ip = '127.0.0.1'
        port = 19997
        time_step = 0.01
        actuator_names = ['LeftMotor', 'RightMotor']
        object_names = ['Bola', 'Dummy']
        robot_names = ['DifferentialDriveRobot']
        scene_path = './Cenario.ttt'
         # Connect to the V-REP continuous server
        vrep.simxFinish(-1)
        self.clientID = vrep.simxStart(ip, port, True, True, -500000, 5)
        if self.clientID != -1: # if we connected successfully
            print ('Connected to remote API server')
        else:
            raise Exception()
        self._configure_environment(scene_path)
        # -------Setup the simulation
        vrep.simxSynchronous(self.clientID, True) #if we need to be syncronous
        # move simulation ahead one time step
        vrep.simxSynchronousTrigger(self.clientID)

        vrep.simxSetFloatingParameter(self.clientID,
                vrep.sim_floatparam_simulation_time_step,
                time_step, # specify a simulation time step
                vrep.simx_opmode_oneshot)

        vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
        time.sleep(.05)

        self.get_handles(actuator_names, robot_names, object_names)

        self.time_step = time_step
        #todo check if observation_space and action_space are correct
        shape = len(robot_names)*5 + len(object_names)*4
        self.observation_space = spaces.Box(low=0, high=1700, shape=[shape])
        shape = len(robot_names)*2
        self.action_space = spaces.Box(low=-10, high=10, shape=[shape])

        self.getSimulationState()

        robot_pos = np.array(self.state['DifferentialDriveRobot'][0:2])
        target_pos = np.array(self.state['Bola'][0:2])
        self.old_distance = np.linalg.norm(robot_pos - target_pos)
        self.init_orientation = self.get_orientation('DifferentialDriveRobot')
Ejemplo n.º 15
0
def listener():
    global joint_handles
    global clientID

    rospy.init_node('controller', anonymous=True)

    vrep.simxFinish(-1)  # just in case, close all opened connections
    clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 500,
                              5)  # Connect to V-REP
    if clientID != -1:
        print('Connected to remote API server')

        vrep.simxSynchronous(clientID, True)

        joint_names = [
            'Shoulder_Joint', 'Upper_Arm_Joint', 'Lower_Arm_Joint',
            'Wrist_Joint', 'Palm_Joint'
        ]
        joint_handles = [
            vrep.simxGetObjectHandle(clientID, name,
                                     vrep.simx_opmode_blocking)[1]
            for name in joint_names
        ]

        dt = .1
        vrep.simxSetFloatingParameter(clientID,
                                      vrep.sim_floatparam_simulation_time_step,
                                      dt, vrep.simx_opmode_oneshot)
        vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)

        # Wrist position and orientation
        q = vrep.simxGetObjectQuaternion(clientID, joint_handles[4], -1,
                                         vrep.simx_opmode_streaming)
        pos = vrep.simxGetObjectPosition(clientID, joint_handles[4],
                                         joint_handles[0],
                                         vrep.simx_opmode_streaming)

    rospy.Subscriber("/quats", quats, callback)

    rospy.spin()
    vrep.simxGetPingTime(clientID)
    vrep.simxFinish(clientID)
Ejemplo n.º 16
0
    def simulation_setup(self):

        # # --------------------- Setup the simulation

        vrep.simxSynchronous(self.clientID, True)

        # Mentioning the joint names to later extract joint handles for the same
        joint_names = ['youBotArmJoint0', 'youBotArmJoint1', 'youBotArmJoint2', 'youBotArmJoint3', 'youBotArmJoint4',\
                       'youBotGripperJoint1', 'youBotGripperJoint2']

        # Need to get name of the links here before obtaining there handles
        link_names = ['Sphere']

        # joint target velocities discussed below
        joint_target_velocities = np.ones(len(joint_names)) * 10000000.0

        # get the handles for each link of the arm
        self.link_handles = [
            vrep.simxGetObjectHandle(self.clientID, name, self.block_mode)[1]
            for name in link_names
        ]

        self.top_handle = self.link_handles[0]

        # self.base_handle = vrep.simxGetObjectHandle(self.clientID, base_name, self.block_mode)[1]

        # By default, V-REP uses argument 3 = -1  to get positions in the world frame -  So the base handle becomes -1
        self.base_handle = -1

        # get the handles for each joint and set up streaming
        self.joint_handles = [
            vrep.simxGetObjectHandle(self.clientID, name, self.block_mode)[1]
            for name in joint_names
        ]

        self.dt = 0.02
        vrep.simxSetFloatingParameter(
            self.clientID, vrep.sim_floatparam_simulation_time_step, self.dt,
            vrep.simx_opmode_oneshot)  # specify a simulation time step
Ejemplo n.º 17
0
    def startSim(self, clientID, screen=True):
        #I need the simulator stopped in order to be started
        retSimStop = vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
        if retSimStop != simx_return_ok :
            print "simulation couldnt be stopped!"
        else:
            print "simulation stopped!"

        #setting the physics engine
        retSetPhyEngine = vrep.simxSetIntegerParameter(clientID, vrep.sim_intparam_dynamic_engine, bulletEngine, vrep.simx_opmode_oneshot_wait)
        if retSetPhyEngine != simx_return_ok:
            print "unable to set the physical engine"
        else:
            print "physical engine correctly setted"

        if int(self.sysConf.getProperty("physics enable?"))==1:
            vrep.simxSetBooleanParameter(clientID,sim_boolparam_dynamics_handling_enabled,True,vrep.simx_opmode_oneshot)
        else:
            vrep.simxSetBooleanParameter(clientID,sim_boolparam_dynamics_handling_enabled,False,vrep.simx_opmode_oneshot)

        #settig simulation speed
        if self.speedmodifier > 0:
            vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_speedmodifier, self.speedmodifier, vrep.simx_opmode_oneshot_wait)

        #settig simulation step
        retSetTimeStep = vrep.simxSetFloatingParameter(clientID,vrep.sim_floatparam_simulation_time_step, self.simulationTimeStepDT, vrep.simx_opmode_oneshot_wait)
        if retSetTimeStep != simx_return_ok :
            print "problems setting time step"
        else:
            print "time step setted!"

        #vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_realtime_simulation,1,vrep.simx_opmode_oneshot_wait)

        #sync mode configuration
        if self.synchronous:
            vrep.simxSynchronous(clientID,True)

        #light mode configuration
        if not screen:
            vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_visible_layers,2,vrep.simx_opmode_oneshot_wait)
            #vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_display_enabled,0,vrep.simx_opmode_oneshot_wait)


        #start simulation
        error=vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot)
        if int(self.sysConf.getProperty("blank screen?"))==1:
            vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_display_enabled,0,vrep.simx_opmode_oneshot_wait)
        return error
def sim_robot(robotType,funcType,reloadrobotsim,robDataFileName,\
            Tmax=0.,inpfn=None,trialclamp=False,Tperiod=0.,Tclamp=0.,simdt=0.001):

    if reloadrobotsim:
        with contextlib.closing(
                shelve.open(robDataFileName+'_'+robotType+'.shelve', 'r')
                ) as data_dict:
            robtrange = data_dict['robtrange']
            rateEvolveProbe = data_dict['angles']
            #torqueArray = data_dict['torque']              # no need to load this
    else:
        if 'robot2' in funcType:
            N = 4                                           # 4-dim system
            joint_names = ['shoulder', 'elbow']
        if 'rr_' in funcType:
            N =7
            joint_names = ['s0','s1','e0','e1','w0','w1','w2'] # 7-dim system
        else:
            N = 2                                           # 2-dim system
            joint_names = ['shoulder']
        zerosNby2 = np.zeros(N//2)
        ###################################### VREP robot arm #####################################
        if robotType == 'V-REP':
            robotdt = .02                                       # we run the robot simulator faster as vrep is slow and dynamics is smooth
                                                                #  and interpolate when feeding to nengo
            import vrep
            # close any open connections
            vrep.simxFinish(-1) 
            # Connect to the V-REP continuous server
            portnum = 19997
            clientID = vrep.simxStart('127.0.0.1', portnum, True, True, 500, 5) 

            if clientID != -1: # if we connected successfully 
                print ('Connected to V-REP remote API server on port',portnum)

                # --------------------- Setup the simulation 

                vrep.simxSynchronous(clientID,True)

                # get handles to each joint
                joint_handles = [vrep.simxGetObjectHandle(clientID, 
                    name, vrep.simx_opmode_blocking)[1] for name in joint_names]

                # set vrep time step
                vrep.simxSetFloatingParameter(clientID, 
                        vrep.sim_floatparam_simulation_time_step,
                        robotdt,
                        vrep.simx_opmode_oneshot)


                # start simulation in blocking mode
                vrep.simxStartSimulation(clientID,
                        vrep.simx_opmode_blocking)
                simrun = True

                robtrange = np.arange(0,Tmax,robotdt)
                rateEvolveProbe = np.zeros(shape=(len(robtrange),N))
                torqueArray = np.zeros(shape=(len(robtrange),N//2))
                for it,t in enumerate(robtrange):
                    torquet = inpfn(t)[N//2:]               # zeros to dq/dt, torque to dv/dt
                    torqueArray[it,:] = torquet
                    
                    if trialclamp and (t%Tperiod)>(Tperiod-Tclamp):
                        if simrun:
                            # stop the vrep simulation
                            vrep.simxStopSimulation(clientID,
                                    vrep.simx_opmode_blocking)
                            simrun = False
                        rateEvolveProbe[it,:N//2] = zerosNby2
                        rateEvolveProbe[it,N//2:] = zerosNby2
                    else:
                        if not simrun:
                            # start simulation in blocking mode
                            vrep.simxStartSimulation(clientID,
                                    vrep.simx_opmode_blocking) 
                            simrun = True
                        # apply the torque to the vrep arm
                        # vrep has a crazy way of setting the torque:
                        #  the torque is applied in target velocity direction
                        #   until target velocity is reached,
                        #  so we need to set the sign of the target velocity correct,
                        #   with its value very high so that it is never reached,
                        #   and then set the torque magnitude as desired.
                        for ii,joint_handle in enumerate(joint_handles): 
                            # first we set the target velocity sign same as the torque sign
                            _ = vrep.simxSetJointTargetVelocity(clientID,
                                    joint_handle,
                                    np.sign(torquet[ii])*9e4,           # target velocity
                                    vrep.simx_opmode_blocking)
                            if _ !=0 : raise Exception()
                            
                            # second we set the torque to abs value desired
                            vrep.simxSetJointForce(clientID, 
                                    joint_handle,
                                    abs(torquet[ii]),                   # 2D torque to apply i.e. \vec{u}(t)
                                    vrep.simx_opmode_blocking)
                            if _ !=0 : raise Exception()

                        # step vrep simulation by a time step
                        vrep.simxSynchronousTrigger(clientID)

                        # get updated joint angles and velocity from vrep
                        q = np.zeros(len(joint_handles))                # 2D output \vec{x}(t)
                        v = np.zeros(len(joint_handles))
                        for ii,joint_handle in enumerate(joint_handles): 
                            # get the joint angles 
                            _, q[ii] = vrep.simxGetJointPosition(clientID,
                                    joint_handle,
                                    vrep.simx_opmode_blocking)
                            if _ !=0 : raise Exception()
                            # get the joint velocity
                            _, v[ii] = vrep.simxGetObjectFloatParameter(clientID,
                                    joint_handle,
                                    2012,                               # parameter ID for angular velocity of the joint
                                    vrep.simx_opmode_blocking)
                            if _ !=0 : raise Exception()
                        rateEvolveProbe[it,:N//2] = q
                        rateEvolveProbe[it,N//2:] = v
                    
                    if it%1000==0:
                        print(it,'time steps, i.e.',t,'s of vrep sim are over.')

                # stop the vrep simulation
                vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)

                # send a blocking command, so that all previous commands finish
                #  redundant perhaps, since stop simulation is also blocking
                vrep.simxGetPingTime(clientID)

                # close the V-REP connection
                vrep.simxFinish(clientID)

            else:
                raise Exception('Connection to V-REP remote API server failed')
                sys.exit(1)
            myarm = None
        ######################################### PENDULUM ARM ######################################
        elif robotType == 'pendulum':
            robotdt = simdt                                             # pendulum simulation is fast enough, no need of slower robotdt
            if funcType in ('robot1_gravity','robot1XY_gravity'):
                from arm_1link_gravity import evolveFns,armXY,armAngles                
            elif funcType in ('robot1_gravity_interpol','robot1XY_gravity_interpol'):
                from arm_1link_gravity_interpol import evolveFns,armXY,armAngles                
            elif funcType in ('robot2_gravity_interpol'):
                from arm_2link_gravity_interpol import evolveFns,armXY,armAngles                
            elif funcType == 'robot2_todorov':
                from arm_2link_todorov import evolveFns,armXY,armAngles
            elif funcType in ('robot2_todorov_gravity','robot2XY_todorov_gravity') or 'rr_' in funcType:
                from arm_2link_todorov_gravity import evolveFns,armXY,armAngles
            elif funcType in ('acrobot2_gravity','acrobot2XY_gravity'):
                from acrobot_2link import evolveFns,armXY,armAngles
            else:
                raise Exception('Choose one- or two-link robot')
            
            if 'rr_' in funcType:
                csv_data = pd.read_csv('../dataset/baxter_traj.csv')
                robtrange = csv_data['time'].values
            else: robtrange = np.arange(0,Tmax,robotdt)
            torqueArray = np.zeros(shape=(len(robtrange),N//2))

            qzero = np.zeros(N//2)                                      # angles and angvelocities at start are subtracted below
            dqzero = np.zeros(N//2)                                     # start from zero, fully downward position initially
            q = np.copy(qzero)                                          # must copy, else pointer is used
            dq = np.copy(dqzero)

            if 'XY' in funcType:                                        # for XY robot, return (positions,angvelocities), though angles are evolved
                rateEvolveProbe = np.zeros(shape=(len(robtrange),N+N//2))
                def set_robot_state(angles,velocities):
                    rateEvolveProbe[it,:N] = armXY(angles)
                    rateEvolveProbe[it,N:] = (velocities-dqzero)
            else:                                                       # for usual robot return (angles,angvelocities)
                rateEvolveProbe = np.zeros(shape=(len(robtrange),N))
                def set_robot_state(angles,velocities):
                    #rateEvolveProbe[it,:N//2] = ((angles+np.pi)%(2*np.pi)-qzero)
                                                                        # wrap angles within -pi and pi
                                                                        # not arctan(tan()) as it returns within -pi/2 and pi/2
                    rateEvolveProbe[it,:N//2] = (angles-qzero)          # don't wrap angles, limit them or use trials so they don't run away
                                                                        # subtract out the start position
                    rateEvolveProbe[it,N//2:] = (velocities-dqzero)                    

#             for it,t in enumerate(robtrange):
#                 if trialclamp and (t%Tperiod)>(Tperiod-Tclamp):
#                     # at the end of each trial, bring arm to start position
#                     q = np.copy(qzero)                                  # must copy, else pointer is used
#                     dq = np.copy(dqzero)
#                 else:
                    #torquet = inpfn(t)[N//2:]                          # torque to dv/dt ([:N//2] has only zeros for dq/dt) -- for ff+rec
#                     torquet = inpfn(t)                                  # torque to dv/dt -- for rec
#                     torqueArray[it,:] = torquet
#                     qdot,dqdot = evolveFns(q,dq,torquet)
#                     q += qdot*robotdt
#                     dq += dqdot*robotdt
            if 'rr_' not in funcType:
                set_robot_state(q,dq)
            else:rateEvolveProbe = csv_data[['right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']].values

        else:
            raise Exception('Choose robotType')
            sys.exit(1)
        
        with contextlib.closing(
                # 'c' opens for read/write, creating if non-existent
                shelve.open(robDataFileName+'_'+robotType+'.shelve', 'c', protocol=-1)
                ) as data_dict:
            data_dict['robtrange'] = robtrange
            data_dict['angles'] = rateEvolveProbe
            data_dict['torque'] = torqueArray

    return robtrange,rateEvolveProbe,evolveFns,armAngles
Ejemplo n.º 19
0
def Init_vrep(cID, tstep):
    '''vrep参数的初始化'''
    vrep.simxSetFloatingParameter(cID, vrep.sim_floatparam_simulation_time_step, tstep, vrep.simx_opmode_oneshot)
    vrep.simxSynchronous(cID, True)
    vrep.simxStartSimulation(cID, vrep.simx_opmode_oneshot)
Ejemplo n.º 20
0
def runSimulation():

    # Creates map from json file
    aMap = Map.Map("P25_X.json", "P25_26_traj.json")
    allObstacles = []

    # Writes obstacles to file
    allObstacles.append(aMap.bounding_polygon)
##    for obstacle in aMap.obstacles:
##        allObstacles.append(obstacle)
    writeToFile(allObstacles, "track.txt")

##    rightWall = vrep.simxGetObjectHandle(clientID, "rightWall", vrep.simx_opmode_blocking)
##    topWall = vrep.simxGetObjectHandle(clientID, "topWall", vrep.simx_opmode_blocking)
##    leftWall = vrep.simxGetObjectHandle(clientID, "leftWall", vrep.simx_opmode_blocking)
##    bottomWall = vrep.simxGetObjectHandle(clientID, "bottomWall", vrep.simx_opmode_blocking)
##    vrep.simxSetObjectFloatParameter(clientID, rightWall, 

    # Plans the path
    #path = findPathKP(aMap) # Kinematic Point
    #path = findPathDP(aMap) # Dynamic Point
    #path = findPathDD(aMap) # Differential Drive
##    path = findPathKC(aMap) # Kinematic Car
    formation = Formation(Map.Map("P26_X.json", "P25_26_traj.json").formation_positions)    
    paths = formationAcquisition(Map.Map("P26_X.json", "P25_26_traj.json"), formation)
    acquisitionLength = len(paths[0])
    print(acquisitionLength)
    print(len(paths[1]))
    paths = formationManeuvering(paths, Map.Map("P26_X.json", "P25_26_traj.json"), formation)


    # Reads path from file
    #path = readPath("DP_P3.txt")

    # Initial values
    startNodes = []
    gammas = []
    for path in paths:
        print("hello")
        startNodes.append(path[len(path) - 1])
        if len(path[len(path) - 1].vel) == 0:
            gammas.append(path[len(path) - 1].orientation)
        else:
            gammas.append(velToAng(path[len(path) - 1].vel))
    posZ = 0.025

    # ---------------------- Connects to V-rep
    # Close any open connections
    vrep.simxFinish(-1)
    # Connect to the V-REP continuous server
    clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 500, 5)

    if clientID != -1: # if we connected successfully
        print('Connected to remote API server')

    # --------------------- Setup the simulation

    vrep.simxSynchronous(clientID, True)

    objectNames = []
    numberOfAgents = 10
    objectHandles = [None] * numberOfAgents
    for i in range(numberOfAgents):
        objectNames.append("bubbleRob" + str(i))

        # get the handle for the bubbleRobs
        print(objectHandles[i])
        print(startNodes)
        _, objectHandles[i] = vrep.simxGetObjectHandle(clientID, objectNames[i], vrep.simx_opmode_blocking)

        _ = vrep.simxSetObjectPosition(clientID, objectHandles[i], -1, [startNodes[i].x/10, startNodes[i].y/10, posZ], vrep.simx_opmode_oneshot)
        _ = vrep.simxSetObjectOrientation(clientID, objectHandles[i], -1, [0, 0, gammas[i]],vrep.simx_opmode_oneshot)

    leaderName = "bubbleRob"
    _, leaderHandle = vrep.simxGetObjectHandle(clientID, leaderName, vrep.simx_opmode_blocking)

    _ = vrep.simxSetObjectPosition(clientID, leaderHandle, -1, [aMap.traj_x[0]/10, aMap.traj_y[0]/10, posZ], vrep.simx_opmode_oneshot)
    _ = vrep.simxSetObjectOrientation(clientID, objectHandles[i], -1, [0, 0, aMap.traj_theta[0]],vrep.simx_opmode_oneshot)

    dt = 1
    vrep.simxSetFloatingParameter(clientID,vrep.sim_floatparam_simulation_time_step, dt, vrep.simx_opmode_oneshot)

    # --------------------- Start the simulation

    # start our simulation in lockstep with our code
    vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)

    count = 0

    while count < len(paths[0]): # run for 1 simulated second

        if count >= acquisitionLength:
            gamma = aMap.traj_theta[count - acquisitionLength]
            _ = vrep.simxSetObjectPosition(clientID, leaderHandle, -1, [aMap.traj_x[count - acquisitionLength]/10, aMap.traj_y[count - acquisitionLength]/10, posZ], vrep.simx_opmode_oneshot)
            _ = vrep.simxSetObjectOrientation(clientID, leaderHandle, -1, [0, 0, gamma], vrep.simx_opmode_oneshot)
        
        for pathNo in range(len(paths)):
            currentNode = paths[pathNo][count]
            if len(currentNode.vel) == 0:
                gamma = currentNode.orientation
            else:
                gamma = velToAng(currentNode.vel)

            _ = vrep.simxSetObjectPosition(clientID, objectHandles[pathNo], -1, [currentNode.x/10, currentNode.y/10, posZ], vrep.simx_opmode_oneshot)
            _ = vrep.simxSetObjectOrientation(clientID, objectHandles[pathNo], -1, [0, 0, gamma], vrep.simx_opmode_oneshot)

        # move simulation ahead one time step
        vrep.simxSynchronousTrigger(clientID)
        count += dt

    # Pause simulation
    vrep.simxPauseSimulation(clientID, vrep.simx_opmode_oneshot)
    time.sleep(3)

    # Stop simulation
    vrep.simxStopSimulation(clientID,
            vrep.simx_opmode_blocking)

    # Close the connection to V-REP:
    vrep.simxFinish(clientID)
Ejemplo n.º 21
0
    def __init__(self, episode_len=None):

        self.clientID = None
        # Start V-REP connection
        try:
            vrep.simxFinish(-1)
            print("Connecting to simulator...")
            self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True,
                                           5000, 5)
            if self.clientID == -1:
                print("Failed to connect to remote API Server")
                self.vrep_exit()
        except KeyboardInterrupt:
            self.vrep_exit()
            return

        self.episode_len = episode_len
        self.timestep = 0
        self.dt = .001
        self.propellers = [
            'rotor1thrust', 'rotor2thrust', 'rotor3thrust', 'rotor4thrust'
        ]
        self.quad_name = 'Quadricopter'
        self.scene_name = 'quad_innerloop.ttt'
        self.setpoint_delta = np.array(
            [0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0.])
        self.quad_handle = None

        self.pos_start = np.zeros(3)
        self.euler_start = np.zeros(3)
        self.vel_start = np.zeros(3)
        self.angvel_start = np.zeros(3)
        self.pos_old = self.pos_start
        self.euler_old = self.euler_start
        self.vel_old = self.vel_start
        self.angvel_old = self.angvel_start
        self.pos_new = self.pos_old
        self.euler_new = self.euler_old
        self.vel_new = self.vel_old
        self.angvel_new = self.angvel_old

        ob_high = np.array([
            10., 10., 10., 20., 20., 20., 21., 21., 21., 21., 21., 21., 10.,
            10., 10., 20., 20., 20., 21., 21., 21., 21., 21., 21.
        ])
        ob_low = -ob_high
        self.observation_space = Box(low=ob_low,
                                     high=ob_high,
                                     dtype=np.float32)
        ac_high = np.array([20., 20., 20., 20.])
        ac_low = np.zeros(4)
        self.action_space = Box(low=ac_low, high=ac_high, dtype=np.float32)

        print("Setting simulator to async mode...")
        vrep.simxSynchronous(self.clientID, True)
        vrep.simxSetFloatingParameter(
            self.clientID,
            vrep.sim_floatparam_simulation_time_step,
            self.dt,  # specify a simulation time step
            vrep.simx_opmode_oneshot)

        # Load V-REP scene
        print("Loading scene...")
        vrep.simxLoadScene(self.clientID, self.scene_name, 0xFF,
                           vrep.simx_opmode_blocking)

        # Get quadrotor handle
        err, self.quad_handle = vrep.simxGetObjectHandle(
            self.clientID, self.quad_name, vrep.simx_opmode_blocking)

        # Initialize quadrotor position and orientation
        vrep.simxGetObjectPosition(self.clientID, self.quad_handle, -1,
                                   vrep.simx_opmode_streaming)
        vrep.simxGetObjectOrientation(self.clientID, self.quad_handle, -1,
                                      vrep.simx_opmode_streaming)
        vrep.simxGetObjectVelocity(self.clientID, self.quad_handle,
                                   vrep.simx_opmode_streaming)
Ejemplo n.º 22
0
    def reset(self):
        vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot_wait)
        time.sleep(0.1)

        # Setup V-REP simulation
        print("Setting simulator to async mode...")
        vrep.simxSynchronous(self.clientID, True)
        vrep.simxSetFloatingParameter(
            self.clientID,
            vrep.sim_floatparam_simulation_time_step,
            self.dt,  # specify a simulation time step
            vrep.simx_opmode_oneshot)

        # Load V-REP scene
        print("Loading scene...")
        vrep.simxLoadScene(self.clientID, self.scene_name, 0xFF,
                           vrep.simx_opmode_blocking)

        # Get quadrotor handle
        err, self.quad_handle = vrep.simxGetObjectHandle(
            self.clientID, self.quad_name, vrep.simx_opmode_blocking)

        # Initialize quadrotor position and orientation
        vrep.simxGetObjectPosition(self.clientID, self.quad_handle, -1,
                                   vrep.simx_opmode_streaming)
        vrep.simxGetObjectOrientation(self.clientID, self.quad_handle, -1,
                                      vrep.simx_opmode_streaming)
        vrep.simxGetObjectVelocity(self.clientID, self.quad_handle,
                                   vrep.simx_opmode_streaming)

        # Start simulation
        vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)

        # Initialize rotors
        print("Initializing propellers...")
        for i in range(len(self.propellers)):
            vrep.simxClearFloatSignal(self.clientID, self.propellers[i],
                                      vrep.simx_opmode_oneshot)

        # Get quadrotor initial position and orientation
        err, self.pos_start = vrep.simxGetObjectPosition(
            self.clientID, self.quad_handle, -1, vrep.simx_opmode_buffer)
        err, self.euler_start = vrep.simxGetObjectOrientation(
            self.clientID, self.quad_handle, -1, vrep.simx_opmode_buffer)
        err, self.vel_start, self.angvel_start = vrep.simxGetObjectVelocity(
            self.clientID, self.quad_handle, vrep.simx_opmode_buffer)

        self.pos_start = np.asarray(self.pos_start)
        self.euler_start = np.asarray(self.euler_start) * 10.
        self.vel_start = np.asarray(self.vel_start)
        self.angvel_start = np.asarray(self.angvel_start)

        self.pos_old = self.pos_start
        self.euler_old = self.euler_start
        self.vel_old = self.vel_start
        self.angvel_old = self.angvel_start
        self.pos_new = self.pos_old
        self.euler_new = self.euler_old
        self.vel_new = self.vel_old
        self.angvel_new = self.angvel_old

        self.pos_error = (self.pos_start + self.setpoint_delta[0:3]) \
                         - self.pos_new
        self.euler_error = (self.euler_start + self.setpoint_delta[3:6]) \
                           - self.euler_new
        self.vel_error = (self.vel_start + self.setpoint_delta[6:9]) \
                         - self.vel_new
        self.angvel_error = (self.angvel_start + self.setpoint_delta[9:12]) \
                            - self.angvel_new

        self.pos_error_all = self.pos_error
        self.euler_error_all = self.euler_error

        self.init_f = 5.8

        self.propeller_vels = [
            self.init_f, self.init_f, self.init_f, self.init_f
        ]

        self.timestep = 1

        return self.get_state()
Ejemplo n.º 23
0
 def set_custom_dt(self, custom_dt):
     _ = vrep.simxSetFloatingParameter(self.client_id, vrep.sim_floatparam_simulation_time_step, custom_dt,
                                       vrep.simx_opmode_blocking)
Ejemplo n.º 24
0
def get_sample_distribution():

    # functions = {}
    # args = {}
    # real_fun(functions)
    # 1-15 are examples along a sphere, one at top of sphere, one in middle, one in bottom,
    # and then three rings at centred .25d, .5d, .75d with radius = 2
    key = 'Traj{}'.format(traj_no)

    # state_normalizers = np.zeros((1, 2))
    # action_normalizers = np.zeros((1, 2))
    # states = np.genfromtxt('trajectories/target/Traj{}state.txt'.format(traj_no), delimiter=',', dtype=np.float32)[1:]
    # actions = np.genfromtxt('trajectories/target/Traj{}action.txt'.format(traj_no),delimiter=' ', dtype=np.float32)[1:]

    states, actions = get_all_files()
    states[isnan(states)] = 0
    actions[isnan(actions)] = 0
    # print(states.shape)
    # print(actions.shape)

    N, T, du = np.asarray(actions).shape  # Close all threads, in case
    _, _, dx = np.asarray(states).shape

    #
    # ilqr = Control(traj_no, max_iter=3)
    # X, U, cost = ilqr.ilqr(states[0,:], actions)

    samples = sample_dist(states, actions)
    raw_input()
    target_vel = X[:,24:]

    vrep.simxFinish(-1)
    cid = connect()
    vrep.simxLoadScene(cid, 'scene/arm_no_control.ttt', 0, mode())
    dt = .01
    vrep.simxSetFloatingParameter(cid,
                                  vrep.sim_floatparam_simulation_time_step,
                                  dt,  # specify a simulation time step
                                  vrep.simx_opmode_oneshot)
    vrep.simxStartSimulation(cid, vrep.simx_opmode_streaming)


    joint_names = ['redundantRob_joint' + str(x) for x in range(1, 8)]
    joint_handles = [vrep.simxGetObjectHandle(cid,
                                              name,
                                              vrep.simx_opmode_oneshot_wait)[1]
                     for name in joint_names]


    # # get handle for target and set up streaming
    _, target_handle = vrep.simxGetObjectHandle(cid,
                                                'redundantRob_manipSphere',
                                                vrep.simx_opmode_oneshot_wait)
    #
    _, self_handle = vrep.simxGetObjectHandle(cid, 'Rectangle',
                                              vrep.simx_opmode_oneshot_wait)
    #
    _, target_pos = vrep.simxGetObjectPosition(cid,
                                               target_handle, -1,
                                               vrep.simx_opmode_oneshot_wait)

    real_args(args, target_pos)

    target_move(cid, target_handle, False, functions[key],
                           args[key])

    for t in range(T):
        #
        # for j in range(du):
        #     if np.sign(actions[t,j]) * target_vel[t,j] < 0:
        #         target_vel[j] = target_vel[j] * -1

        [vrep.simxSetJointTargetVelocity(cid, joint_handles[j], target_vel[t,j],
                                         vrep.simx_opmode_streaming) for j in range(du)]
        [vrep.simxSetJointForce(cid, joint_handles[j], actions[t,j],
                                vrep.simx_opmode_streaming) for j in range(du)]
        vrep.simxSynchronousTrigger(cid)
        raw_input()
        print('here')

    vrep.simxSynchronousTrigger(cid)
    vrep.simxStopSimulation(cid, vrep.simx_opmode_streaming)
    vrep.simxFinish(cid)
Ejemplo n.º 25
0
        center = np.array([0, .15, 0.8])
        dist = .1
        num_targets = 5
        target_positions = np.array([
            [dist*np.cos(theta), dist*np.cos(theta), dist*np.sin(theta)] \
                    + center for theta in np.linspace(0, np.pi, num_targets)])

        # define variables to share with nengo
        q = np.zeros(len(joint_handles))
        dq = np.zeros(len(joint_handles))

        # --------------------- Start the simulation

        dt = .01
        vrep.simxSetFloatingParameter(clientID, 
                vrep.sim_floatparam_simulation_time_step, 
                dt, # specify a simulation time step
                vrep.simx_opmode_oneshot)

        # start our simulation in lockstep with our code
        vrep.simxStartSimulation(clientID,
                vrep.simx_opmode_blocking)

        count = 0
        track_hand = []
        track_target = []
        target_index = 0
        change_target_time = dt*300

        # NOTE: main loop starts here ---------------------------------------------
        while count < len(target_positions) * change_target_time: # run this many seconds
Ejemplo n.º 26
0
        vrep.simxSynchronous(clientID, True)

        #Jaco joint_names
        joint_names = ['Jaco_joint%d' % i for i in range(1, 7)]

        # Get the handles for each joint
        joint_handles = [
            vrep.simxGetObjectHandle(clientID, name,
                                     vrep.simx_opmode_blocking)[1]
            for name in joint_names
        ]

        vrep.simxSetFloatingParameter(
            clientID,
            vrep.sim_floatparam_simulation_time_step,
            dt,  # specify a simulation time step
            vrep.simx_opmode_oneshot)

        # strt our simulation in lockstep with our code
        vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)

        k = 0

        # For 300 torques values retrieved from the csv files

        while k < 700:
            #Send the torques values
            uu = torques[k]

            for ii, joint_handle in enumerate(joint_handles):
Ejemplo n.º 27
0
def main():
    # Start V-REP connection
    try:
        vrep.simxFinish(-1)
        print("Connecting to simulator...")
        clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
        if clientID == -1:
            print("Failed to connect to remote API Server")
            vrep_exit(clientID)
    except KeyboardInterrupt:
        vrep_exit(clientID)
        return

    # Setup V-REP simulation
    print("Setting simulator to async mode...")
    vrep.simxSynchronous(clientID, True)
    dt = .001
    vrep.simxSetFloatingParameter(
        clientID,
        vrep.sim_floatparam_simulation_time_step,
        dt,  # specify a simulation time step
        vrep.simx_opmode_oneshot)

    # Load V-REP scene
    print("Loading scene...")
    vrep.simxLoadScene(clientID, scene_name, 0xFF, vrep.simx_opmode_blocking)

    # Get quadrotor handle
    err, quad_handle = vrep.simxGetObjectHandle(clientID, quad_name,
                                                vrep.simx_opmode_blocking)

    # Initialize quadrotor position and orientation
    vrep.simxGetObjectPosition(clientID, quad_handle, -1,
                               vrep.simx_opmode_streaming)
    vrep.simxGetObjectOrientation(clientID, quad_handle, -1,
                                  vrep.simx_opmode_streaming)

    # Start simulation
    vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)

    # Initialize rotors
    print("Initializing propellers...")
    for i in range(len(propellers)):
        vrep.simxClearFloatSignal(clientID, propellers[i],
                                  vrep.simx_opmode_oneshot)

    # Get quadrotor initial position and orientation
    err, pos_old = vrep.simxGetObjectPosition(clientID, quad_handle, -1,
                                              vrep.simx_opmode_buffer)
    err, euler_old = vrep.simxGetObjectOrientation(clientID, quad_handle, -1,
                                                   vrep.simx_opmode_buffer)

    pos_old = np.asarray(pos_old)
    euler_old = np.asarray(euler_old) * 10.

    pos_start = pos_old

    # hyper parameters

    n_input = 6
    n_forces = 4
    #n_action = 8
    hidden = 256
    batch_size = 512
    learning_rate = .01
    eps = 0.1
    gamma = 0.9

    init_f = 7.

    net = DQN(n_input + n_forces, hidden, 1)

    optim = torch.optim.Adam(net.parameters(), lr=learning_rate)
    state = [0 for i in range(n_input)]
    state = torch.from_numpy(np.asarray(state,
                                        dtype=np.float32)).view(-1, n_input)

    propeller_vels = [init_f, init_f, init_f, init_f]

    extended_state = torch.cat(
        (state, torch.from_numpy(np.asarray([propeller_vels],
                                            dtype=np.float32))), 1)
    memory = ReplayMemory(10000)

    while (vrep.simxGetConnectionId(clientID) != -1):

        # epsilon greedy
        r = random.random()
        if r > eps:
            delta_forces = generate_forces(net, extended_state, learning_rate)
        else:
            delta_forces = [float(random.randint(-1, 1)) for i in range(4)]
        # Set propeller thrusts
        print("Setting propeller thrusts...")
        propeller_vels = apply_forces(propeller_vels, delta_forces)

        # Send propeller thrusts
        print("Sending propeller thrusts...")
        [
            vrep.simxSetFloatSignal(clientID, prop, vels,
                                    vrep.simx_opmode_oneshot)
            for prop, vels in zip(propellers, propeller_vels)
        ]

        # Trigger simulator step
        print("Stepping simulator...")
        vrep.simxSynchronousTrigger(clientID)

        # Get quadrotor initial position and orientation
        err, pos_new = vrep.simxGetObjectPosition(clientID, quad_handle, -1,
                                                  vrep.simx_opmode_buffer)
        err, euler_new = vrep.simxGetObjectOrientation(clientID, quad_handle,
                                                       -1,
                                                       vrep.simx_opmode_buffer)

        pos_new = np.asarray(pos_new)
        euler_new = np.asarray(euler_new) * 10
        #euler_new[2]/=100

        valid = is_valid_state(pos_start, pos_new, euler_new)
        if valid:
            next_state = torch.FloatTensor(
                np.concatenate([euler_new, pos_new - pos_old]))
            #next_state = torch.FloatTensor(euler_new )

            next_extended_state = torch.FloatTensor(
                [np.concatenate([next_state,
                                 np.asarray(propeller_vels)])])
        else:
            next_state = None
            next_extended_state = None

        reward_acc = np.float32(-np.linalg.norm(
            pos_new - pos_old)) if next_state is not None else np.float32(-10.)

        reward_alpha = np.float32(
            -np.fabs(euler_new[0])
        ) if not check_quad_flipped(euler_new) else np.float32(-10.)
        reward_beta = np.float32(
            -np.fabs(euler_new[1])
        ) if not check_quad_flipped(euler_new) else np.float32(-10.)
        reward_gamma = np.float32(
            -np.fabs(euler_new[2])
        ) if not check_quad_flipped(euler_new) else np.float32(-5.)

        reward_alpha_acc = np.float32(
            -np.fabs(euler_new[0] - euler_old[0])
        ) if not check_quad_flipped(euler_new) else np.float32(-10.)
        reward_beta_acc = np.float32(
            -np.fabs(euler_new[1] - euler_old[1])
        ) if not check_quad_flipped(euler_new) else np.float32(-10.)
        reward_gamma_acc = np.float32(
            -np.fabs(euler_new[2] - euler_old[2])
        ) if not check_quad_flipped(euler_new) else np.float32(-5.)

        reward = reward_alpha + reward_beta + reward_gamma + reward_acc + reward_alpha_acc * 10. + reward_beta_acc * 10. + reward_gamma_acc * 10.

        if not check_quad_flipped(euler_new) and not valid:
            print('Success.')
            reward = 10.

        #reward_pos=-np.linalg.norm(pos_new-pos_start)

        if reward < -10.:
            reward = -10.
        reward = np.float32(reward)
        memory.push(
            extended_state,
            torch.from_numpy(np.asarray([delta_forces], dtype=np.float32)),
            next_extended_state, torch.from_numpy(np.asarray([[reward]])))

        state = next_state
        extended_state = next_extended_state
        pos_old = pos_new
        euler_old = euler_new
        print(propeller_vels)
        print("\n")

        # Perform one step of the optimization (on the target network)

        #DQN_update2(net, memory, batch_size, gamma, optim)

        if not valid:
            DQN_update2(net, memory, batch_size, gamma, optim)
            print('reset')
            # reset
            reset(clientID)
            print("Loading scene...")
            vrep.simxLoadScene(clientID, scene_name, 0xFF,
                               vrep.simx_opmode_blocking)

            # Setup V-REP simulation
            print("Setting simulator to async mode...")
            vrep.simxSynchronous(clientID, True)
            dt = .0005
            vrep.simxSetFloatingParameter(
                clientID,
                vrep.sim_floatparam_simulation_time_step,
                dt,  # specify a simulation time step
                vrep.simx_opmode_oneshot)
            # Get quadrotor handle
            err, quad_handle = vrep.simxGetObjectHandle(
                clientID, quad_name, vrep.simx_opmode_blocking)

            # Initialize quadrotor position and orientation
            vrep.simxGetObjectPosition(clientID, quad_handle, -1,
                                       vrep.simx_opmode_streaming)
            vrep.simxGetObjectOrientation(clientID, quad_handle, -1,
                                          vrep.simx_opmode_streaming)

            # Start simulation
            vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)

            # Get quadrotor initial position and orientation
            err, pos_old = vrep.simxGetObjectPosition(clientID, quad_handle,
                                                      -1,
                                                      vrep.simx_opmode_buffer)
            err, euler_old = vrep.simxGetObjectOrientation(
                clientID, quad_handle, -1, vrep.simx_opmode_buffer)
            euler_old = euler_old
            pos_old = np.asarray(pos_old)
            euler_old = np.asarray(euler_old) * 10.
            pos_start = pos_old

            state = [0 for i in range(n_input)]
            state = torch.FloatTensor(np.asarray(state)).view(-1, n_input)

            propeller_vels = [init_f, init_f, init_f, init_f]

            extended_state = torch.cat(
                (state, torch.FloatTensor(np.asarray([propeller_vels]))), 1)
            print('duration: ', len(memory))
            memory = ReplayMemory(10000)
Ejemplo n.º 28
0
def setSimulationTimeStep(clientID, dt = 0.01):
    
    vrep.simxSetFloatingParameter(clientID, 
                                  vrep.sim_floatparam_simulation_time_step, 
                                  dt, 
                                  oneshot)
Ejemplo n.º 29
0
def getTrajectories():
    print ('Program started')

    #Start Grabbing Handles
    functions = {}
    args = {}
    real_fun(functions)
    #1-15 are examples along a sphere, one at top of sphere, one in middle, one in bottom,
    #and then three rings at centred .25d, .5d, .75d with radius = 2

    k = functions.keys()
    k.sort()

    iter = 1
    for key in k:
        firstPass = True
        state_file = open('trajectories/target/{}.txt'.format(key+"state"),'w+')
        action_file = open('trajectories/target/{}.txt'.format(key+"action"),'w+')

        vrep.simxFinish(-1)  # just in case, close all opened connections
        time.sleep(1)
        cid = connect()
        time.sleep(1)

        vrep.simxLoadScene(cid,'scene/arm_2.ttt',0,mode())
        dt = .01
        vrep.simxSetFloatingParameter(cid,
                vrep.sim_floatparam_simulation_time_step,
                dt, # specify a simulation time step
                vrep.simx_opmode_oneshot)
        vrep.simxStartSimulation(cid,vrep.simx_opmode_streaming)
        runtime = 0

        joint_names = ['redundantRob_joint'+str(x) for x in range(1,8)]
        # print(joint_names)
        # # joint target velocities discussed below
        # joint_target_velocities = np.ones(len(joint_names)) * 10000.0
        #
        # # get the handles for each joint and set up streaming
        joint_handles = [vrep.simxGetObjectHandle(cid,
            name, vrep.simx_opmode_oneshot_wait)[1] for name in joint_names]
        # print(joint_handles)
        # # get handle for target and set up streaming
        _, target_handle = vrep.simxGetObjectHandle(cid,
                        'redundantRob_manipSphere', vrep.simx_opmode_oneshot_wait)
        #
        _, self_handle = vrep.simxGetObjectHandle(cid,'Rectangle',vrep.simx_opmode_oneshot_wait)
        #
        _, target_pos = vrep.simxGetObjectPosition(cid,
                                                   target_handle, -1, vrep.simx_opmode_oneshot_wait)

        #This prepared the data
        real_args(args, target_pos)
        print(key)

        while(runtime < 0.4):
            target_move(cid,target_handle, firstPass, functions[key], args[key])
            #vrep.simxSynchronous(cid, False)
            vrep.simxGetPingTime(cid)
            firstPass = controller_motor(cid,target_handle,self_handle, joint_handles, firstPass)

            commands = [vrep.simxGetJointForce(cid, joint, vrep.simx_opmode_oneshot_wait)[1] for joint in
                        joint_handles]

            joint_pos = [vrep.simxGetObjectPosition(cid, joint, vrep.sim_handle_parent, vrep.simx_opmode_oneshot_wait)[1] for joint
                         in joint_handles]

            joint_vel = [vrep.simxGetObjectFloatParameter(cid, joint, 2012, vrep.simx_opmode_blocking)[1]
                         for joint in joint_handles]

            target_rel = vrep.simxGetObjectPosition(cid, target_handle, joint_handles[-1],
                                                    vrep.simx_opmode_oneshot_wait)[1]

            joint_state = np.append(np.asarray(joint_pos).flatten(),target_rel + joint_vel)
            # joint_state = np.append(np.asarray(joint_pos).flatten(),joint_vel)
            # print(np.asarray(joint_state).shape)
            # print(joint_state)

            state_file.write("{}\n".format(",".join(
                [str(x) for x in joint_state])))
            action_file.write(
                "{}\n".format(" ".join([str(x)[1:-2] for x in commands])))
            runtime += dt

            vrep.simxSynchronousTrigger(cid)
            firstPass = False

        vrep.simxStopSimulation(cid, vrep.simx_opmode_streaming)
        vrep.simxSynchronousTrigger(cid)
        vrep.simxFinish(cid)
        firstPass = True
        print("meow")
Ejemplo n.º 30
0
def runSimulation():

    # Creates map from json file
    aMap = Map("P3.json")
    allObstacles = []

    # Writes obstacles to file
    allObstacles.append(aMap.bounding_polygon)
    for obstacle in aMap.obstacles:
        allObstacles.append(obstacle)
    writeToFile(allObstacles, "track.txt")

    # Plans the path
    #path = findPathKP(aMap) # Kinematic Point
    #path = findPathDP(aMap) # Dynamic Point
    #path = findPathDD(aMap) # Differential Drive
    path = findPathKC(aMap)  # Kinematic Car

    # Reads path from file
    #path = readPath("DP_P3.txt")

    # Initial values
    startNode = path[0]
    if len(startNode.vel) == 0:
        gamma = startNode.orientation
    else:
        gamma = velToAng(startNode.vel)
    posZ = 0.025

    # ---------------------- Connects to V-rep
    # Close any open connections
    vrep.simxFinish(-1)
    # Connect to the V-REP continuous server
    clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 500, 5)

    if clientID != -1:  # if we connected successfully
        print('Connected to remote API server')

    # --------------------- Setup the simulation

    vrep.simxSynchronous(clientID, True)

    objectName = "bubbleRob"

    # get the handle for bubbleRob
    _, objectHandle = vrep.simxGetObjectHandle(clientID, objectName,
                                               vrep.simx_opmode_blocking)

    _ = vrep.simxSetObjectPosition(clientID, objectHandle, -1,
                                   [startNode.x / 10, startNode.y / 10, posZ],
                                   vrep.simx_opmode_oneshot)
    _ = vrep.simxSetObjectOrientation(clientID, objectHandle, -1,
                                      [0, 0, gamma], vrep.simx_opmode_oneshot)

    dt = 1
    vrep.simxSetFloatingParameter(clientID,
                                  vrep.sim_floatparam_simulation_time_step, dt,
                                  vrep.simx_opmode_oneshot)

    # --------------------- Start the simulation

    # start our simulation in lockstep with our code
    vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)

    count = 0

    while count < len(path):  # run for 1 simulated second

        currentNode = path[count]
        if len(currentNode.vel) == 0:
            gamma = currentNode.orientation
        else:
            gamma = velToAng(currentNode.vel)

        _ = vrep.simxSetObjectPosition(
            clientID, objectHandle, -1,
            [currentNode.x / 10, currentNode.y / 10, posZ],
            vrep.simx_opmode_oneshot)
        _ = vrep.simxSetObjectOrientation(clientID, objectHandle, -1,
                                          [0, 0, gamma],
                                          vrep.simx_opmode_oneshot)

        # move simulation ahead one time step
        vrep.simxSynchronousTrigger(clientID)
        count += dt

    # Pause simulation
    vrep.simxPauseSimulation(clientID, vrep.simx_opmode_oneshot)
    time.sleep(3)

    # Stop simulation
    vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)

    # Close the connection to V-REP:
    vrep.simxFinish(clientID)
Ejemplo n.º 31
0
 def _turn_display(self, Value = True):
     if (Value):
         vrep.simxSetFloatingParameter(self.clientID, vrep.sim_boolparam_display_enabled, True, vrep.simx_opmode_blocking)
     else:
         vrep.simxSetFloatingParameter(self.clientID, vrep.sim_boolparam_display_enabled, False, vrep.simx_opmode_blocking)
Ejemplo n.º 32
0
vrep.simxFinish(-1)
# connect to vrep-server
while True:

    clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
    if clientID > -1:
        break
    else:
        time.sleep(0.2)
        print("Failed connecting to remoteAPI server!")
print("Connection success!")

# configuration
# set simulatio time step
vrep.simxSetFloatingParameter(clientID,
                              vrep.sim_floatparam_simulation_time_step, tstep,
                              vrep.simx_opmode_oneshot)
# open synchronous model
vrep.simxSynchronous(clientID, True)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)

# read handles of base and joint

jointHandle = np.zeros((jointNum, ), dtype=np.int)
for i in range(jointNum):
    # print("i:",i)
    _, returnHandle = vrep.simxGetObjectHandle(clientID,
                                               jointName + str(i + 1),
                                               vrep.simx_opmode_blocking)
    jointHandle[i] = returnHandle
_, baseHandle = vrep.simxGetObjectHandle(clientID, baseName,
Ejemplo n.º 33
0
    def __init__(self):
        #建立通信
        vrep.simxFinish(-1)
        # 每隔0.2s检测一次,直到连接上V-rep
        while True:
            self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True,
                                           5000, 5)
            if self.clientID != -1:
                break
            else:
                time.sleep(0.1)
                print("Failed connecting to remote API server!")
        print("Connection success!")
        # # 设置机械臂仿真步长,为了保持API端与V-rep端相同步长
        vrep.simxSetFloatingParameter(self.clientID, vrep.sim_floatparam_simulation_time_step, self.dt,\
                                      vrep.simx_opmode_oneshot)
        # # 然后打开同步模式
        vrep.simxSynchronous(self.clientID, False)
        vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)
        # vrep.simxSynchronousTrigger(self.clientID)
        #获取 joint 句柄
        self.robot1_jointHandle = np.zeros((self.jointNum, ),
                                           dtype=np.int)  # joint 句柄
        self.robot2_jointHandle = np.zeros((self.jointNum, ),
                                           dtype=np.int)  # joint 句柄
        for i in range(self.jointNum):
            _, returnHandle = vrep.simxGetObjectHandle(
                self.clientID, self.jointName + str(i + 1),
                vrep.simx_opmode_blocking)
            self.robot1_jointHandle[i] = returnHandle
        for i in range(self.jointNum):
            _, returnHandle = vrep.simxGetObjectHandle(
                self.clientID, self.jointName + str(i + 4),
                vrep.simx_opmode_blocking)
            self.robot2_jointHandle[i] = returnHandle
        # 获取 Link 句柄
        self.robot1_linkHandle = np.zeros((self.jointNum, ),
                                          dtype=np.int)  # link 句柄
        self.robot2_linkHandle = np.zeros((self.jointNum, ),
                                          dtype=np.int)  # link 句柄
        for i in range(self.jointNum):
            _, returnHandle = vrep.simxGetObjectHandle(
                self.clientID, self.linkName + str(i + 1),
                vrep.simx_opmode_blocking)
            self.robot1_linkHandle[i] = returnHandle
        for i in range(self.jointNum):
            _, returnHandle = vrep.simxGetObjectHandle(
                self.clientID, self.linkName + str(i + 4),
                vrep.simx_opmode_blocking)
            self.robot2_linkHandle[i] = returnHandle
        # 获取碰撞句柄
        _, self.robot1_collisionHandle = vrep.simxGetCollisionHandle(
            self.clientID, 'Collision_robot1', vrep.simx_opmode_blocking)
        _, self.robot2_collisionHandle = vrep.simxGetCollisionHandle(
            self.clientID, 'Collision_robot2', vrep.simx_opmode_blocking)

        # 获取距离句柄
        # _,self.mindist_robot1_Handle = vrep.simxGetDistanceHandle(self.clientID,'dis_robot1',vrep.simx_opmode_blocking)
        # _,self.mindist_robot2_Handle = vrep.simxGetDistanceHandle(self.clientID,'dis_robot2', vrep.simx_opmode_blocking)
        # _,self.mindist_robots_Handle = vrep.simxGetDistanceHandle(self.clientID,'robots_dist', vrep.simx_opmode_blocking)
        _, self.robot1_goal_Handle = vrep.simxGetDistanceHandle(
            self.clientID, 'robot1_goal', vrep.simx_opmode_blocking)
        _, self.robot2_goal_Handle = vrep.simxGetDistanceHandle(
            self.clientID, 'robot2_goal', vrep.simx_opmode_blocking)
        # 获取末端句柄
        _, self.end1_Handle = vrep.simxGetObjectHandle(
            self.clientID, 'end', vrep.simx_opmode_blocking)
        _, self.end2_Handle = vrep.simxGetObjectHandle(
            self.clientID, 'end0', vrep.simx_opmode_blocking)

        _, self.goal1_Handle = vrep.simxGetObjectHandle(
            self.clientID, 'goal_1', vrep.simx_opmode_blocking)
        _, self.goal2_Handle = vrep.simxGetObjectHandle(
            self.clientID, 'goal_2', vrep.simx_opmode_blocking)
        print('Handles available!')
        _, self.goal_1 = vrep.simxGetObjectPosition(self.clientID,
                                                    self.goal1_Handle, -1,
                                                    vrep.simx_opmode_blocking)
        _, self.goal_2 = vrep.simxGetObjectPosition(self.clientID,
                                                    self.goal2_Handle, -1,
                                                    vrep.simx_opmode_blocking)
        del self.goal_1[2]
        del self.goal_2[2]
        self.goal_1 = np.array(self.goal_1)
        self.goal_2 = np.array(self.goal_2)
        self.jointConfig1 = np.zeros((self.jointNum, ))
        self.jointConfig2 = np.zeros((self.jointNum, ))
        for i in range(self.jointNum):
            _, jpos = vrep.simxGetJointPosition(self.clientID,
                                                self.robot1_jointHandle[i],
                                                vrep.simx_opmode_blocking)
            self.jointConfig1[i] = jpos
        for i in range(self.jointNum):
            _, jpos = vrep.simxGetJointPosition(self.clientID,
                                                self.robot2_jointHandle[i],
                                                vrep.simx_opmode_blocking)
            self.jointConfig2[i] = jpos

        _, collision1 = vrep.simxReadCollision(self.clientID,
                                               self.robot1_collisionHandle,
                                               vrep.simx_opmode_blocking)
        _, collision2 = vrep.simxReadCollision(self.clientID,
                                               self.robot2_collisionHandle,
                                               vrep.simx_opmode_blocking)
        _, pos1 = vrep.simxGetObjectPosition(self.clientID, self.end1_Handle,
                                             -1, vrep.simx_opmode_blocking)
        _, pos2 = vrep.simxGetObjectPosition(self.clientID, self.end2_Handle,
                                             -1, vrep.simx_opmode_blocking)
        _, d1 = vrep.simxReadDistance(self.clientID, self.robot1_goal_Handle,
                                      vrep.simx_opmode_blocking)
        _, d2 = vrep.simxReadDistance(self.clientID, self.robot2_goal_Handle,
                                      vrep.simx_opmode_blocking)
        for i in range(self.jointNum):
            _, returnpos = vrep.simxGetObjectPosition(
                self.clientID, self.robot1_linkHandle[i], -1,
                vrep.simx_opmode_blocking)

        for i in range(self.jointNum):
            _, returnpos = vrep.simxGetObjectPosition(
                self.clientID, self.robot2_linkHandle[i], -1,
                vrep.simx_opmode_blocking)
        print('programing start!')
        vrep.simxSynchronousTrigger(self.clientID)  # 让仿真走一步