def stop(self):
"""
Sets the motors velocities to 0.
"""
vrep.simxSetJointTargetVelocity(self.clientID, self.motors_handle["left"], 0, vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(self.clientID, self.motors_handle["right"], 0, vrep.simx_opmode_streaming)
time.sleep(0.1)
def set_right_velocity(self, vel):
"""
Sets the velocity on the right motor.
Args:
vel: The velocity to be applied in the motor (rad/s)
"""
self.last_right_vel = vel
vrep.simxSetJointTargetVelocity(self.clientID, self.motors_handle["right"], vel, vrep.simx_opmode_streaming)
def set_velocity(self, V, W):
"""
Sets a linear and a angular velocity on the robot.
Args:
V: Linear velocity (m/s) to be applied on the robot along its longitudinal axis.
W: Angular velocity (rad/s) to be applied on the robot along its axis of rotation, positive in the counter-clockwise direction.
"""
left_vel = (V - W*(self.ROBOT_WIDTH/2))/self.WHEEL_RADIUS
right_vel = (V + W*(self.ROBOT_WIDTH/2))/self.WHEEL_RADIUS
vrep.simxSetJointTargetVelocity(self.clientID, self.motors_handle["left"], left_vel, vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(self.clientID, self.motors_handle["right"], right_vel, vrep.simx_opmode_streaming)
def execute_motion(self, vel_sets):
#vrep.simxPauseSimulation(self.CLIENT_ID,0)
vrep.simxSynchronous(self.CLIENT_ID, True)
vrep.simxStartSimulation(self.CLIENT_ID, vrep.simx_opmode_oneshot)
for dur, vels in vel_sets:
vrep.simxPauseCommunication(self.CLIENT_ID, 1)
for key in range(1, 19):
vrep.simxSetJointTargetVelocity(self.CLIENT_ID,
self.MOTOR_HANDLES[key],
vels[key],
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(self.CLIENT_ID, 0)
time.sleep(dur)
vrep.simxSynchronousTrigger(self.CLIENT_ID)
vrep.simxPauseCommunication(self.CLIENT_ID, 1)
for key in range(1, 19):
vrep.simxSetJointTargetVelocity(self.CLIENT_ID,
self.MOTOR_HANDLES[key], 0,
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(self.CLIENT_ID, 0)
vrep.simxSynchronousTrigger(self.CLIENT_ID)
def set_joint_target_velocity(clientID, handle, target_velocity):
response = vrep.simxSetJointTargetVelocity(clientID, handle,
target_velocity,
vrep.simx_opmode_oneshot)
def set_joint_target_velocity(self, handle, target_velocity):
response = vrep.simxSetJointTargetVelocity(self.client_id, handle,
target_velocity,
vrep.simx_opmode_oneshot)
assert response[0] != vrep.simx_return_ok, "cannot set joint target vel"
print "Error = ",e
exit(1)
MOTOR_HANDLES[i] = handle
def getPos():
angles = []
for i in MOTOR_HANDLES.keys():
_,angle = vrep.simxGetJointPosition(CLIENT_ID,MOTOR_HANDLES[i],vrep.simx_opmode_oneshot_wait)
angles.append(math.degrees(angle))
return angles
def setPos(writ):
for key,val in writ.items():
vrep.simxSetJointTargetPosition(CLIENT_ID,MOTOR_HANDLES[key],math.radians(val),vrep.simx_opmode_oneshot_wait)
connectVrep()
get_handles()
state = getPos()
print(state)
angle = 5
errCode,motor2 =vrep.simxGetObjectHandle(CLIENT_ID,'j_shoulder_r',vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(CLIENT_ID,motor2,-angle,vrep.simx_opmode_oneshot)
time.sleep(3)
state = getPos()
print(state)