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)
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
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]
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
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')
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
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)
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!')
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
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)
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})
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')
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)
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
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
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)
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)
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)
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()
def set_custom_dt(self, custom_dt):
_ = vrep.simxSetFloatingParameter(self.client_id, vrep.sim_floatparam_simulation_time_step, custom_dt,
vrep.simx_opmode_blocking)
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)
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
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):
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)
def setSimulationTimeStep(clientID, dt = 0.01):
vrep.simxSetFloatingParameter(clientID,
vrep.sim_floatparam_simulation_time_step,
dt,
oneshot)
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")
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)
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)
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,
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) # 让仿真走一步
