def stop(self):
#set divers to stop mode
force = 0
err_code = vrep.simxSetJointForce(self.clientID,
self.left_front_handle, force,
vrep.simx_opmode_oneshot)
err_code = vrep.simxSetJointForce(self.clientID, self.left_back_handle,
force, vrep.simx_opmode_oneshot)
err_code = vrep.simxSetJointForce(self.clientID,
self.right_back_handle, force,
vrep.simx_opmode_oneshot)
err_code = vrep.simxSetJointForce(self.clientID,
self.right_front_handle, force,
vrep.simx_opmode_oneshot)
force = 10
for h in self.side_handles:
err_code = vrep.simxSetJointForce(self.clientID, h, force,
vrep.simx_opmode_oneshot)
#break
self.leftvelocity = 10
self.rightvelocity = 10
vrep.simxSetJointTargetVelocity(self.clientID, self.left_front_handle,
self.leftvelocity,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(self.clientID, self.left_back_handle,
self.leftvelocity,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(self.clientID, self.right_back_handle,
self.rightvelocity,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(self.clientID, self.right_front_handle,
self.rightvelocity,
vrep.simx_opmode_streaming)
def setSpeed(self, side, speed, force=500):
sim.simxSetJointForce(self.clientID,
self.motor[side],
force,
sim.simx_opmode_streaming)
sim.simxSetJointTargetVelocity(self.clientID,
self.motor[side],
speed,
sim.simx_opmode_streaming)
def go(self):
#set divers to go mode
force = 10
err_code = vrep.simxSetJointForce(self.clientID,
self.left_front_handle, force,
vrep.simx_opmode_oneshot)
err_code = vrep.simxSetJointForce(self.clientID, self.left_back_handle,
force, vrep.simx_opmode_oneshot)
err_code = vrep.simxSetJointForce(self.clientID,
self.right_back_handle, force,
vrep.simx_opmode_oneshot)
err_code = vrep.simxSetJointForce(self.clientID,
self.right_front_handle, force,
vrep.simx_opmode_oneshot)
force = 0
for h in self.side_handles:
err_code = vrep.simxSetJointForce(self.clientID, h, force,
vrep.simx_opmode_oneshot)
def set_torque(set_torque, torque):
error = 9
#start= timer()
if set_torque == 'ab3' or set_torque == 0:
while (error != 0):
error = sim.simxSetJointForce(clientID, int(angles_handler[0]),
torque, sim.simx_opmode_streaming)
if torque > 0:
set_target_vel(int(angles_handler[0]), 'p')
else:
set_target_vel(int(angles_handler[0]), 'n')
elif set_torque == 'bc3' or set_torque == 1:
while (error != 0):
error = sim.simxSetJointForce(clientID, int(angles_handler[1]),
torque, sim.simx_opmode_streaming)
if torque > 0:
set_target_vel(int(angles_handler[1]), 'p')
else:
set_target_vel(int(angles_handler[1]), 'n')
elif set_torque == 'cd3' or set_torque == 2:
while (error != 0):
error = sim.simxSetJointForce(clientID, int(angles_handler[2]),
torque, sim.simx_opmode_streaming)
if torque > 0:
set_target_vel(int(angles_handler[2]), 'p')
else:
set_target_vel(int(angles_handler[2]), 'n')
elif set_torque == 'ab4' or set_torque == 3:
while (error != 0):
error = sim.simxSetJointForce(clientID, int(angles_handler[3]),
torque, sim.simx_opmode_streaming)
if torque > 0:
set_target_vel(int(angles_handler[3]), 'p')
else:
set_target_vel(int(angles_handler[3]), 'n')
elif set_torque == 'bc4' or set_torque == 4:
while (error != 0):
error = sim.simxSetJointForce(clientID, int(angles_handler[4]),
torque, sim.simx_opmode_streaming)
if torque > 0:
set_target_vel(int(angles_handler[4]), 'p')
else:
set_target_vel(int(angles_handler[4]), 'n')
elif set_torque == 'cd4' or set_torque == 5:
while (error != 0):
error = sim.simxSetJointForce(clientID, int(angles_handler[5]),
torque, sim.simx_opmode_streaming)
if torque > 0:
set_target_vel(int(angles_handler[5]), 'p')
else:
set_target_vel(int(angles_handler[5]), 'n')
elif set_torque == 'ab1' or set_torque == 6:
while (error != 0):
error = sim.simxSetJointForce(clientID, int(angles_handler[6]),
torque, sim.simx_opmode_streaming)
if torque > 0:
set_target_vel(int(angles_handler[6]), 'p')
else:
set_target_vel(int(angles_handler[6]), 'n')
elif set_torque == 'bc1' or set_torque == 7:
while (error != 0):
error = sim.simxSetJointForce(clientID, int(angles_handler[7]),
torque, sim.simx_opmode_streaming)
if torque > 0:
set_target_vel(int(angles_handler[7]), 'p')
else:
set_target_vel(int(angles_handler[7]), 'n')
elif set_torque == 'cd1' or set_torque == 8:
while (error != 0):
error = sim.simxSetJointForce(clientID, int(angles_handler[8]),
torque, sim.simx_opmode_streaming)
if torque > 0:
set_target_vel(int(angles_handler[8]), 'p')
else:
set_target_vel(int(angles_handler[8]), 'n')
#print('another error is '+str(error))
elif set_torque == 'ab2' or set_torque == 9:
while (error != 0):
error = sim.simxSetJointForce(clientID, int(angles_handler[9]),
torque, sim.simx_opmode_streaming)
if torque > 0:
set_target_vel(int(angles_handler[9]), 'p')
else:
set_target_vel(int(angles_handler[9]), 'n')
elif set_torque == 'bc2' or set_torque == 10:
while (error != 0):
error = sim.simxSetJointForce(clientID, int(angles_handler[10]),
torque, sim.simx_opmode_streaming)
if torque > 0:
set_target_vel(int(angles_handler[10]), 'p')
else:
set_target_vel(int(angles_handler[10]), 'n')
elif set_torque == 'cd2' or set_torque == 11:
while (error != 0):
error = sim.simxSetJointForce(clientID, int(angles_handler[11]),
torque, sim.simx_opmode_streaming)
if torque > 0:
set_target_vel(int(angles_handler[11]), 'p')
else:
set_target_vel(int(angles_handler[11]), 'n')
control_force = sol.value(U[0, 0])
#
#control_force = 400
if np.sign(control_force) >= 0:
set_force = control_force
set_velocity = 9999
else:
set_force = -control_force
set_velocity = -9999
# 控制命令需要同时方式，故暂停通信，用于存储所有控制命令一起发送
vrep.simxPauseCommunication(clientID, True)
vrep.simxSetJointTargetVelocity(clientID, jointHandle, set_velocity,
vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID, jointHandle, set_force,
vrep.simx_opmode_oneshot)
#vrep.simxSetJointForce(clientID,jointHandle,set_force,vrep.simx_opmode_oneshot);
# vrep.simxSetJointTargetPosition(clientID, jointHandle, 100, vrep.simx_opmode_oneshot)
# vrep.simxSetJointTargetPosition(clientID, jointHandle[1], 5/RAD2DEG, vrep.simx_opmode_oneshot)
# vrep.simxSetJointTargetVelocity(clientID, jointHandle[2], 500/RAD2DEG, vrep.simx_opmode_oneshot)
# vrep.simxSetJointTargetVelocity(clientID, jointHandle[3], 500/RAD2DEG, vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
# ***
lastCmdTime = currCmdTime # 记录当前时间
vrep.simxSynchronousTrigger(clientID) # 进行下一步
vrep.simxGetPingTime(clientID) # 使得该仿真步走完
def set_maximum_force(self, force):
s.simxSetJointForce(
self._id, self._handle, force, self._def_op_mode)
speed = 50
a = 1
b = 10
while time.time() - startTime < 100:
ret, [i, j, k] = sim.simxGetObjectOrientation(
clientID, rob_hand, -1,
sim.simx_opmode_buffer) # Initialize streaming
ret, [x, y, z] = sim.simxGetObjectPosition(
clientID, rob_hand, -1,
sim.simx_opmode_buffer) # Initialize streaming
if ret == sim.simx_return_ok: # After initialization of streaming, it will take a few ms before the first value arrives, so check the return code
print(y)
torq = 0 * j + b * y
torq = max(-0.01, torq)
torq = min(0.01, torq)
sim.simxSetJointForce(clientID, motor_hand, abs(torq),
sim.simx_opmode_oneshot)
print(torq)
if (torq > 0): speed = 900
elif (torq < 0): speed = -900
else: speed = 0
sim.simxSetJointTargetVelocity(clientID, motor_hand, speed,
sim.simx_opmode_oneshot)
time.sleep(0.01)
# Now send some data to CoppeliaSim in a non-blocking fashion:
sim.simxAddStatusbarMessage(clientID, 'Hello CoppeliaSim!',
sim.simx_opmode_oneshot)
# Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
sim.simxGetPingTime(clientID)
#control_force = 400
if np.sign(control_force[0]) >= 0:
set_force[0] = control_force[0]
set_velocity[0] = 9999
else:
set_force[0] = -control_force[0]
set_velocity[0] = -9999
if np.sign(control_force[1]) >= 0:
set_force[1] = control_force[1]
set_velocity[1] = 9999
else:
set_force[1] = -control_force[1]
set_velocity[1] = -9999
# 控制命令需要同时方式，故暂停通信，用于存储所有控制命令一起发送
vrep.simxPauseCommunication(clientID, True)
vrep.simxSetJointTargetVelocity(clientID, jointHandle[1], set_velocity[0], vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID, jointHandle[1], set_force[0], vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(clientID, jointHandle[2], set_velocity[1], vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID, jointHandle[2], set_force[1], vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
# ***
print(currCmdTime-lastCmdTime)
lastCmdTime=currCmdTime # 记录当前时间
vrep.simxSynchronousTrigger(clientID) # 进行下一步
vrep.simxGetPingTime(clientID) # 使得该仿真步走完
def setGripper(self, side, speed, force=500):
sim.simxSetJointForce(self.clientID, self.gripperHandle[side], force, sim.simx_opmode_streaming)
sim.simxSetJointTargetVelocity(self.clientID,
self.gripperHandle[side],
speed,
sim.simx_opmode_streaming)
def control_velocity(motor_velocity, motor_torque):
sim.simxSetJointForce(clientID, motor_handle, motor_torque,
sim.simx_opmode_blocking)
sim.simxSetJointTargetVelocity(clientID, motor_handle, motor_velocity,
sim.simx_opmode_blocking)
#第一条腿
rf_rot_1_pos = 0
rf_rot_2_pos = 0
rf_rot_3_pos = 0
#第二条腿
lb_rot_1_pos = 0
lb_rot_2_pos = 0
lb_rot_3_pos = 0
#第三条腿
lf_rot_1_pos = 0
lf_rot_2_pos = 0
lf_rot_3_pos = 0
#第四条腿
#置电机力矩
rec = sim.simxSetJointForce(clientID, rb_rot_3, rb_rot_3_force,
sim.simx_opmode_blocking)
rec = sim.simxSetJointForce(clientID, rf_rot_3, rf_rot_3_force,
sim.simx_opmode_blocking)
rec = sim.simxSetJointForce(clientID, rb_rot_2, rb_rot_2_force,
sim.simx_opmode_blocking)
rec = sim.simxSetJointForce(clientID, rf_rot_2, rf_rot_2_force,
sim.simx_opmode_blocking)
rec = sim.simxSetJointForce(clientID, rb_rot_1, rb_rot_1_force,
sim.simx_opmode_blocking)
rec = sim.simxSetJointForce(clientID, rf_rot_1, rf_rot_1_force,
sim.simx_opmode_blocking)
rec = sim.simxSetJointForce(clientID, lb_rot_3, lb_rot_3_force,
sim.simx_opmode_blocking)
rec = sim.simxSetJointForce(clientID, lf_rot_3, lf_rot_3_force,
sim.simx_opmode_blocking)
rec = sim.simxSetJointForce(clientID, lb_rot_2, lb_rot_2_force,