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,