def onScale(self, name, value):
if (self.clientID != -1):
print("{} : {}".format(name, value))
obj = self.handles[int(name)]
ang = float(value) * math.pi / 180
sim.simxSetJointPosition(self.clientID, obj, ang,
sim.simx_opmode_oneshot)
def init_robot(self):
for h in self.handles:
sim.simxSetJointPosition(self.clientID, h, 0,
sim.simx_opmode_oneshot)
print("termino de inicializar")
_, pos = sim.simxGetObjectPosition(self.clientID, self.j2, -1,
sim.simx_opmode_streaming)
_, pos = sim.simxGetObjectPosition(self.clientID, self.j1, -1,
sim.simx_opmode_streaming)
def plan_motion(self, target_config, steps=1):
traj = np.linspace(start=self.joint_config,
stop=target_config,
num=steps)
for i in range(traj.shape[0]):
for joint in range(len(self.joints)):
# sim.simxSetJointTargetPosition(self.clientID, self.joints[joint], target_config[joint], sim.simx_opmode_oneshot_wait)
sim.simxSetJointPosition(self.clientID, self.joints[joint],
traj[i, joint],
sim.simx_opmode_oneshot_wait)
self.joint_config = self.control
def adjustJoints(self, x):
# pause to load all joints at the same time
sim.simxPauseCommunication(self.clientID, True)
for i in range(len(self.joints)):
sim.simxSetJointPosition(self.clientID, self.joints[i], x[i] * 0.01745329252, sim.simx_opmode_oneshot)
sim.simxPauseCommunication(self.clientID, False)
time.sleep(0.02) # give the simulation time to catch up
# return the distance between tip and target
_, d = sim.simxReadDistance(self.clientID, self.distanceHandle, sim.simx_opmode_blocking)
return d
def set_position(self, coords):
"""Установить положение (для статической сцены)
Не работает для динамической сцены. Для статической вызовает мгновенное перемещение.
При работе с обратной связью следует использовать в режиме синхронизации для предотвращения
получения неактуальных данных.
Parameters
----------
coords : ndarray
Обобщённые координаты
"""
sim.simxPauseCommunication(self.client, 1)
for i in range(JOINT_COUNT):
sim.simxSetJointPosition(self.client, self.joints[i], coords[i],
sim.simx_opmode_oneshot)
sim.simxPauseCommunication(self.client, 0)
if self.synchronous:
self._step_simulation()
def IK_w_rotation(self,
targetPos,
targetRot,
alpha=0.1,
num_iter=500,
threshold=0.005):
for i in range(num_iter):
jacobian, error, curr_config = self.compute_jacobian(
targetPos, targetRot)
if (np.linalg.norm(error) < threshold):
print("taget reached")
return curr_config
error[0, 3:5] = 0
# using jacobian transpose
delta_config = (jacobian.T).dot(error.T).T
config = curr_config + delta_config * alpha
for idx in range(len(self.joints)):
sim.simxSetJointPosition(self.clientID, self.joints[idx],
config[0,
idx], sim.simx_opmode_blocking)
def set_joints(self, theta):
if self.is_connected():
for i in range(6):
sim.simxSetJointPosition(self.clientId, self.joints[i],
theta[i], sim.simx_opmode_oneshot)
time.sleep(0.5)
_, youBotRef = sim.simxGetObjectHandle(clientID, 'youBot_ref',
sim.simx_opmode_blocking)
_, tip = sim.simxGetObjectHandle(clientID, 'youBot_positionTip',
sim.simx_opmode_blocking)
_, target = sim.simxGetObjectHandle(clientID, 'youBot_positionTarget',
sim.simx_opmode_blocking)
minDetection = 0.01
maxDetection = 0.5
# Set Joint Position
print('Set Joint Position')
desiredJ = [0, np.pi / 2, 0, 0, 0]
# Forward Kinematics
for i in range(5):
_ = sim.simxSetJointPosition(clientID, armJoints[i], desiredJ[i],
sim.simx_opmode_streaming)
# Inverse Kinematics to pick up object
_, T = sim.simxGetJointMatrix(clientID, armJoints[0],
sim.simx_opmode_streaming)
print(T)
gripperCommunicationTube = sim.simxtubeOpen(0, 'youBotGripperState', 1)
gripperReturn = im.simTubeWrite(gripperCommunicationTube,
sim.packInt32Table({0}), 32)
# Start Driving
"""
print('Start Driving')
sim.simxSetJointTargetVelocity(clientID, wheelJoints[0], -0.7, sim.simx_opmode_streaming)
def openDoor(doorHandle):
r, pos = sim.simxGetJointPosition(clientID, doorHandle, sim.simx_opmode_streaming)
if (pos == 0):
sim.simxSetJointPosition(clientID, doorHandle, -90 * math.pi / 180, sim.simx_opmode_oneshot)
import sim
import sys
import time
import math
print(__version__)
print('Programa inicio')
sim.simxFinish(-1) # cerrar todas las conexiones
# Conectar a CoppeliaSim
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
print('Conectado al API del servidor remoto')
_, M1 = sim.simxGetObjectHandle(clientID, 'Joint1',
sim.simx_opmode_blocking)
sim.simxSetJointPosition(clientID, M1, 90 * math.pi / 180,
sim.simx_opmode_streaming)
print("velocidad")
time.sleep(10) #dormir unos segundos antes de terminar
# Ahora cerrar la conexion a CoppeliaSim:
sim.simxFinish(clientID)
else:
sys.exit('Fallo conectando al API del servidor remoto')
print('Programa finalizado')
def set_position(self, position):
s.simxSetJointPosition(
self._id, self._handle, position, self._def_op_mode)
def init_robot(self):
for h in self.handles:
sim.simxSetJointPosition(self.clientID, h,0 ,sim.simx_opmode_oneshot)
print("termino de inicializar")
def setJointPosition(self, handle, position, ignoreError = False, initialize = False):
res = sim.simxSetJointPosition(self.clientID, handle, position, sim.simx_opmode_oneshot)
if res!=sim.simx_return_ok and not ignoreError:
print('Failed to set joint angle')
print(res)
print('get object gripper joint 1 ok')
return_code, gripper_joint_2_handle = vrep_sim.simxGetObjectHandle(clientID, 'youBotGripperJoint2', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object gripper joint 2 ok')
return_code, gripper_tip_handle = vrep_sim.simxGetObjectHandle(clientID, 'youBot_positionTip', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object gripper position tip ok')
# Desired joint positions for initialization
desired_arm_joint_angles = [0, 0, 0, 0, 0]
# Initialization all arm joints
for i in range(0,5):
vrep_sim.simxSetJointPosition(clientID, arm_joints_handle[i], desired_arm_joint_angles[i], vrep_sim.simx_opmode_blocking)
# Get world joints handles
world_joints_handle = [-1, -1, -1]
return_code, world_joints_handle[0] = vrep_sim.simxGetObjectHandle(clientID, 'World_X_Joint', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object world joint X ok.')
return_code, world_joints_handle[1] = vrep_sim.simxGetObjectHandle(clientID, 'World_Y_Joint', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object world joint Y ok.')
return_code, world_joints_handle[2] = vrep_sim.simxGetObjectHandle(clientID, 'World_Th_Joint', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object world joint Th ok.')
def apply_control(self, config):
for idx in range(len(self.joints)):
sim.simxSetJointPosition(self.clientID, self.joints[idx],
config[idx], sim.simx_opmode_blocking)
