def wait_until_stop(self, handle, threshold=0.01):
"""
Wait until the operation finishes.
This is a delay function called in order to make sure that
the operation executed has been completed.
:param handle: An int.Handle of the object.
:param threshold: A float. The object position threshold.
If the object positions difference between two time steps is smaller than the threshold,
the execution completes, otherwise the loop continues.
:return: None
"""
while True:
_, pos1 = vrep.simxGetObjectPosition(self.client_id, handle, -1,
vrep.simx_opmode_blocking)
_, quat1 = vrep.simxGetObjectQuaternion(self.client_id, handle, -1,
vrep.simx_opmode_blocking)
time.sleep(0.7)
_, pos2 = vrep.simxGetObjectPosition(self.client_id, handle, -1,
vrep.simx_opmode_blocking)
_, quat2 = vrep.simxGetObjectQuaternion(self.client_id, handle, -1,
vrep.simx_opmode_blocking)
pose1 = pos1 + quat1
pose2 = pos2 + quat2
theta = 0.5 * sqrt(
reduce(lambda x, y: x + y,
map(lambda x, y: (x - y)**2, pose1, pose2)))
if theta < threshold:
return
def get_quaternion(self):
_, quaternion = vrep.simxGetObjectQuaternion(
self.client_id,
self.handle,
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking)
return quaternion
def get_object_info(self, obj_name):
# object info = position, orientation, scale
print "find", obj_name, "in vrep scene"
returnCode, object = vrep.simxGetObjectHandle(
self.clientID, obj_name, vrep.simx_opmode_blocking)
returnCode, obj_pos = vrep.simxGetObjectPosition(
self.clientID, object, self.robot_base_world,
vrep.simx_opmode_oneshot_wait)
returnCode, obj_ori_q = vrep.simxGetObjectQuaternion(
self.clientID, object, self.robot_base_world,
vrep.simx_opmode_oneshot_wait)
returnCode, bb_min_x = vrep.simxGetObjectFloatParameter(
self.clientID, object, 15, vrep.simx_opmode_oneshot_wait)
returnCode, bb_min_y = vrep.simxGetObjectFloatParameter(
self.clientID, object, 16, vrep.simx_opmode_oneshot_wait)
returnCode, bb_min_z = vrep.simxGetObjectFloatParameter(
self.clientID, object, 17, vrep.simx_opmode_oneshot_wait)
returnCode, bb_max_x = vrep.simxGetObjectFloatParameter(
self.clientID, object, 18, vrep.simx_opmode_oneshot_wait)
returnCode, bb_max_y = vrep.simxGetObjectFloatParameter(
self.clientID, object, 19, vrep.simx_opmode_oneshot_wait)
returnCode, bb_max_z = vrep.simxGetObjectFloatParameter(
self.clientID, object, 20, vrep.simx_opmode_oneshot_wait)
obj_sx = bb_max_x - bb_min_x
obj_sy = bb_max_y - bb_min_y
obj_sz = bb_max_z - bb_min_z
obj_scale = [obj_sx, obj_sy, obj_sz]
return obj_pos, obj_ori_q, obj_scale
def get_obj_orien(self): # Initialize IMU err, objHandle = vrep.simxGetObjectHandle(self.clientID, 'ObjectFrame', vrep.simx_opmode_oneshot) err, quadHandle = vrep.simxGetObjectHandle(self.clientID, 'joint1', vrep.simx_opmode_oneshot) # Gets the object orientation err, obj_orien = vrep.simxGetObjectQuaternion(self.clientID, objHandle,quadHandle, vrep.simx_opmode_oneshot) obj_orien_rot = tf.transformations.quaternion_matrix([obj_orien[0], obj_orien[1], obj_orien[2],obj_orien[3]]) # print(obj_orien) return obj_orien_rot
def wait_until_stop(self, handle, threshold=0.01):
while True:
_, pos1 = vrep.simxGetObjectPosition(self.client_id, handle, -1,
vrep.simx_opmode_blocking)
_, quat1 = vrep.simxGetObjectQuaternion(self.client_id, handle, -1,
vrep.simx_opmode_blocking)
time.sleep(0.7)
_, pos2 = vrep.simxGetObjectPosition(self.client_id, handle, -1,
vrep.simx_opmode_blocking)
_, quat2 = vrep.simxGetObjectQuaternion(self.client_id, handle, -1,
vrep.simx_opmode_blocking)
pose1 = pos1 + quat1
pose2 = pos2 + quat2
theta = 0.5 * sqrt(
reduce(lambda x, y: x + y,
map(lambda x, y: (x - y)**2, pose1, pose2)))
if theta < threshold:
return
def getPoints(self, pTrue, pFalse, w, h, s):
""" Get the position and orientation of the sensors. Get the points of the detected objects """
def q2R(x, y, z, w):
""" Transforms a quaternion to a rotation matrix """
R = np.zeros((3, 3))
R[0, 0] = 1 - 2 * (y**2 + z**2)
R[0, 1] = 2 * (x * y - z * w)
R[0, 2] = 2 * (x * z + y * w)
R[1, 0] = 2 * (x * y + z * w)
R[1, 1] = 1 - 2 * (x**2 + z**2)
R[1, 2] = 2 * (y * z - x * w)
R[2, 0] = 2 * (x * z - y * w)
R[2, 1] = 2 * (y * z + x * w)
R[2, 2] = 1 / 2 * (x**2 + y**2)
return R
sPosition, sState, sPoints, sPointsW = [], [], [], []
for i in range(16):
_, position = vrep.simxGetObjectPosition(self.clientID,
self.usensor[i], -1,
vrep.simx_opmode_blocking)
_, quat = vrep.simxGetObjectQuaternion(self.clientID,
self.usensor[i], -1,
vrep.simx_opmode_blocking)
_, state, point, detectedObj, norm = vrep.simxReadProximitySensor(
self.clientID, self.usensor[i], vrep.simx_opmode_buffer)
sPointsW.append(np.linalg.norm(point))
row, col = self.getGridCoord(position[0], position[1], w, h, s)
sPosition.append([row, col])
R = r.random()
pS = [0, 0, 1]
if state:
B = R < pTrue
if B:
state = False
pS = [0, 0, 1]
else:
state = True
pS = point
else:
B = R < pFalse
if B:
state = True
pS = [0, 0, r.random()]
else:
state = False
pS = [0, 0, 1]
sState.append(state)
rotM = np.array(q2R(quat[0], quat[1], quat[2], quat[3]))
pos = np.array(pS)
# Get the position of the detected object in world coordinate
pW = np.matmul(rotM, pos) + position
row, col = self.getGridCoord(pW[0], pW[1], w, h, s)
sPoints.append([row, col])
return sPosition, sState, sPoints
def get_moving_obstacle_pose(self):
handle = self.moving_obstacle_handle
rct = 1
pos = np.zeros(3)
while rct != 0 or np.linalg.norm(pos) < 0.001:
rct, pos = vrep.simxGetObjectPosition(self.clientID, handle, -1,
vrep.simx_opmode_streaming)
rcq, quat = vrep.simxGetObjectQuaternion(
self.clientID, handle, -1, vrep.simx_opmode_streaming)
return np.array(pos), np.array(quat)
def getObjectQuaternion(self,
object_name,
relative2='parent',
initialize=False):
#Changes what it is relative to
if relative2 == 'parent':
relative_handle = vrep.sim_handle_parent
else:
relative_handle = self.handles[self.handle_names.index(relative2)]
if self.clientID == None:
print("Robot not attached to VREP environment")
elif initialize:
quaternion = vrep.simxGetObjectQuaternion(
self.clientID, object_name, relative_handle,
vrep.simx_opmode_streaming) # !CHANGE OPMODE
else:
quaternion = vrep.simxGetObjectQuaternion(
self.clientID, object_name, relative_handle,
vrep.simx_opmode_buffer
) # !CHANGE OPMODE from operationMode=self.opmode
return quaternion #(x, y, z, w)?
def get_end_matrix(self):
_, quaternion = vrep.simxGetObjectQuaternion(
self.client_id,
self.end_handle,
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking)
quaternion = np.array(quaternion)
quaternion = quaternion[[3, 0, 1, 2]]
r = quat2rot(quaternion)
_, position = vrep.simxGetObjectPosition(
self.client_id,
self.end_handle,
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking)
t = np.array(position)
return r_t_to_mat(r, t)
def __init__(self, client_id, name):
self.client_id = client_id
# RG2 gripper
self.gripper = scene_gripper.Gripper(self.client_id,
gripper_name="RG2")
# initialization
self.func = partial(vrep.simxCallScriptFunction,
clientID=self.client_id,
scriptDescription="UR5",
options=vrep.sim_scripttype_childscript,
operationMode=vrep.simx_opmode_blocking)
self.initialization()
# UR5 related
self.joint_handles = [
vrep.simxGetObjectHandle(
self.client_id,
"UR5_joint{}".format(i + 1),
operationMode=vrep.simx_opmode_oneshot_wait)[1]
for i in range(6)
]
res, self.end_handle = vrep.simxGetObjectHandle(
self.client_id,
"UR5_ik_tip",
operationMode=vrep.simx_opmode_blocking)
res, self.obj_handle = vrep.simxGetObjectHandle(
self.client_id,
"cam_calibration",
operationMode=vrep.simx_opmode_blocking)
res, self.target_handle = vrep.simxGetObjectHandle(
self.client_id,
"UR5_ik_target",
operationMode=vrep.simx_opmode_blocking)
_, self.init_position = vrep.simxGetObjectPosition(
self.client_id,
self.target_handle,
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking)
_, self.init_quaternion = vrep.simxGetObjectQuaternion(
self.client_id,
self.target_handle,
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking)
def get_cam_matrixs(self):
mats = []
for i in range(3):
res, position = vrep.simxGetObjectPosition(
self.client_id,
self.camera_handles[i],
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking)
t = np.array(position)
res, quaternion = vrep.simxGetObjectQuaternion(
self.client_id,
self.camera_handles[i],
relativeToObjectHandle=-1,
operationMode=vrep.simx_opmode_blocking)
q = np.array(quaternion)[[3, 0, 1, 2]]
r = quat2rot(q)
mats.append(r_t_to_mat(r, t))
return mats
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 get_M(joints, gripper, clientID):
s = []
a = np.array([[0, 0, 1], [0, 1, 0], [0, 1, 0], [0, 1, 0], [0, 0, 1],
[0, 1, 0]])
x = 0
y = 0
z = 0
vector = [[0], [0], [0]]
# for i in range(0,6):
# result,vector=vrep.simxGetObjectPosition(clientID,joints[i],joints[0],vrep.simx_opmode_blocking)
# if result != vrep.simx_return_ok:
# raise Exception('could not get position')
# q=np.array([[vector[0]],[vector[1]],[vector[2]]])
# s.append(getS(a[i],q)[:,0])
# s=np.array(s).T
result, vector = vrep.simxGetObjectPosition(clientID, gripper, joints[0],
vrep.simx_opmode_blocking)
result, theta_come = vrep.simxGetObjectQuaternion(
clientID, gripper, joints[0], vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object orientation')
qx = theta_come[0]
qy = theta_come[1]
qz = theta_come[2]
qw = theta_come[3]
M = np.array([[
1 - 2 * qy * qy - 2 * qz * qz, 2 * qx * qy - 2 * qz * qw,
2 * qx * qz + 2 * qy * qw, vector[0]
],
[
2 * qx * qy + 2 * qz * qw, 1 - 2 * qx * qx - 2 * qz * qz,
2 * qy * qz - 2 * qx * qw, vector[1]
],
[
2 * qx * qz - 2 * qy * qw, 2 * qy * qz + 2 * qx * qw,
1 - 2 * qx * qx - 2 * qy * qy, vector[2]
], [0, 0, 0, 1]])
return M
def callback(quat):
global joint_handles
global clientID
# Decompose message
wrist = quat.wrist_quat
shoulder = quat.shoulder_quat
elbow_angle_in_rad = quat.elbow_angle_in_rad
# Rotate shoulder quaternion +90 degrees around z-axis
q_shoulder = Quaternion(shoulder.w, shoulder.x, shoulder.y, shoulder.z)
rot = Quaternion(w=0.0, x=0.707, y=0.707, z=0.0)
q_shoulder = rot * q_shoulder * rot.inverse
rot_s = Quaternion(w=0.0, x=0.0, y=0.0, z=1.0)
q_shoulder = rot_s * q_shoulder
elements_s = q_shoulder.elements
# Elbow angle
q_lower = Quaternion(axis=(0.0, 0.0, 1.0), radians=elbow_angle_in_rad)
elements_l = q_lower.elements
q_wrist = Quaternion(wrist.w, wrist.z, wrist.x, wrist.y)
rot_w_y = Quaternion(w=0.707, x=0.0, y=0.707, z=0.0)
rot_w_x = Quaternion(w=0.707, x=0.707, y=0.0, z=0.0)
q_wrist = rot_w_x * rot_w_y * q_wrist * rot_w_y.inverse * rot_w_x.inverse
rot_w = Quaternion(w=0.707, x=0.707, y=0.0, z=0.0)
q_wrist = rot_w * q_wrist
rot_w = Quaternion(w=0.985, x=0.0, y=0.0, z=-0.184)
q_wrist = rot_w * q_wrist
rot_w2 = Quaternion(w=0.0, x=0.0, y=0.0, z=1.0)
q_wrist = rot_w2 * q_wrist
elements_w = q_wrist.elements
# Send orientation to VREP
if not joint_handles is None:
# send shoulder orientation
vrep.simxSetObjectQuaternion(
clientID, joint_handles[0], -1,
[elements_s[1], elements_s[2], elements_s[3], elements_s[0]],
vrep.simx_opmode_oneshot)
# send lower arm orientation
vrep.simxSetObjectQuaternion(
clientID, joint_handles[2], joint_handles[1],
[elements_l[1], elements_l[2], elements_l[3], elements_l[0]],
vrep.simx_opmode_oneshot)
# send wrist orientation
vrep.simxSetObjectQuaternion(
clientID, joint_handles[4], -1,
[elements_w[1], elements_w[2], elements_w[3], elements_w[0]],
vrep.simx_opmode_oneshot)
# get end effector orientation
ret, q = vrep.simxGetObjectQuaternion(clientID, joint_handles[4], -1,
vrep.simx_opmode_buffer)
# calculate end effector position
ret, posb = vrep.simxGetObjectPosition(clientID, joint_handles[0], -1,
vrep.simx_opmode_buffer)
ret, pos = vrep.simxGetObjectPosition(clientID, joint_handles[4], -1,
vrep.simx_opmode_streaming)
print(pos)
end_eff = [pos[0], pos[1], pos[2], q[3], q[0], q[1], q[2]]
end_eff_data = Float32MultiArray()
end_eff_data.data = end_eff
pub.publish(end_eff_data)
import sys
vrep.simxFinish(-1)
clientID = vrep.simxStart('192.168.0.19',19999, True, True, 5000, 5)
if clientID != -1:
print('Conexion Establecida!')
else:
sys.exit('Error: No se puede conectar!')
_, left_motor = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
_, right_motor = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)
_, pioneer_handler = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
while True:
position = vrep.simxGetObjectPosition(clientID, pioneer_handler, -1, vrep.simx_opmode_streaming)
orientation = vrep.simxGetObjectOrientation(clientID, pioneer_handler, -1, vrep.simx_opmode_streaming)
velocity = vrep.simxGetObjectVelocity(clientID, pioneer_handler, vrep.simx_opmode_streaming)
quaternion = vrep.simxGetObjectQuaternion(clientID, pioneer_handler, -1, vrep.simx_opmode_streaming)
#print('p>:\t', position)
#print('o>:\t', orientation)
#print('q>:\t', quaternion)
#print(velocity[1])
RotMat = GetFlatRotationMatrix(orientation[1])
rowdata = np.append(RotMat, position[1])
rowdata = np.append(rowdata, velocity[1])
rowdata = np.append(rowdata, velocity[2])
#print(np.shape(rowdata))
print(rowdata)
vrep.simxFinish(-1)
# Create bounding volume dummies for the joints
BOUNDING_VOL_RADIUS = 0.1
TINY_RADIUS = 0.05
# Initialize variable which let us check if the dummy has been moved
prev_dummy_pos = None
try:
while True:
# Wait for user to say they want to move
input("Waiting for user to set point...")
# Get the desired pose by checking the pose of the user-movable dummy
errorCode, dummy_pos = vrep.simxGetObjectPosition(clientID, target_dummy_handle, -1, vrep.simx_opmode_oneshot_wait)
errorCode, dummy_quaternion = vrep.simxGetObjectQuaternion(clientID, target_dummy_handle, -1, vrep.simx_opmode_oneshot_wait)
# Make sure we don't recalculate if the inverse kinematics dummy is still in the same position
if (dummy_pos == prev_dummy_pos):
continue
prev_dummy_pos = dummy_pos
# Mush the quaternion and pos into a pose matrix!
# Our function takes care of the V-REP quaternion mismatch
pose = quaternion.matrix_from_quaternion(dummy_quaternion) # This function fills in only [0:3,0:3] (orientation) so we have to fill in the last part ourselves
pose[0,3] = dummy_pos[0]
pose[1,3] = dummy_pos[1]
pose[2,3] = dummy_pos[2]
print("Desired Pose: \n{}".format(pose))
# Get theta start (current thetas of robot joints)
# closing_joint_handles.append(get_obj_handle('Barrett_openCloseJoint', vrep.simx_opmode_blocking))
# closing_joint_handles.append(get_obj_handle('Barrett_openCloseJoint0', vrep.simx_opmode_blocking))
cup_handle = get_obj_handle('Cup', vrep.simx_opmode_blocking)
#############################################################
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
print("start simulating")
result, connector_pos = vrep.simxGetObjectPosition(
clientID, connection_handle, -1, vrep.simx_opmode_oneshot_wait)
check_error(result, "connector pos", 1)
print("connector postition: \n", connector_pos)
result, connector_quat = vrep.simxGetObjectQuaternion(
clientID, connection_handle, -1, vrep.simx_opmode_oneshot_wait)
check_error(result, "connector quat", 1)
print("connector_quat: \n", connector_quat)
# get cup position
result, cup_pos = vrep.simxGetObjectPosition(clientID, cup_handle, -1,
vrep.simx_opmode_oneshot_wait)
check_error(result, "cup position", 1)
print("cup postition: ", cup_pos)
result, cup_quat = vrep.simxGetObjectQuaternion(clientID, cup_handle, -1,
vrep.simx_opmode_oneshot_wait)
check_error(result, "cup quat", 1)
print("cup quat: ", cup_quat)
target_pos = [-0.1939985603094101, 8.508563041687012e-05, 0.6511231660842896]
target_pos_2 = [0.06673718988895416, -0.3410583734512329, 0.5191779136657715]
cup_pos_1 = [-0.375, 0.06, 0.5717]
def run_simulation(self):
self.set_num_steps()
vrep.simxSynchronous(self._clientID,True);
vrep.simxStartSimulation(self._clientID,vrep.simx_opmode_oneshot);
for simStep in range (self._num_steps):
self._pid = set_target_thetas(self._num_steps, self._pid,self._experiment,self._simulator,simStep)
for joint in range(6):
errorCode, self._theta[joint] = vrep.simxGetJointPosition(self._clientID,self._arm_joints[joint],vrep.simx_opmode_blocking)
self._linearVelocity[joint] = self._pid[joint].get_velocity(math.degrees(self._theta[joint]))/self._convertdeg2rad
vrep.simxSetJointTargetVelocity(self._clientID,self._arm_joints[joint],self._linearVelocity[joint],vrep.simx_opmode_oneshot)
vrep.simxSynchronousTrigger(self._clientID)
vrep.simxGetPingTime(self._clientID)
self._positions.append(vrep.simxGetObjectPosition(self._clientID,self._arm_joints[5],-1,vrep.simx_opmode_blocking)[1] + vrep.simxGetObjectOrientation(self._clientID,self._arm_joints[5],-1,vrep.simx_opmode_blocking)[1] + vrep.simxGetObjectPosition(self._clientID,self._cube,-1,vrep.simx_opmode_blocking)[1] + vrep.simxGetObjectQuaternion(self._clientID,self._cube,-1,vrep.simx_opmode_blocking)[1])
vrep.simxStopSimulation(self._clientID, vrep.simx_opmode_blocking)
vrep.simxFinish(self._clientID)
saveStats(self._experiment,self._current_iteration, self._physics_engine,self._simulator, self._positions)
time.sleep(0.5)
vrep.simxSetJointTargetPosition(clientID, jointHandles[5], theta[5], vrep.simx_opmode_oneshot)
time.sleep(2)
result,vector=vrep.simxGetObjectPosition(clientID,jointHandles[5],jointHandles[0],vrep.simx_opmode_blocking)
result2,theta_come2 =vrep.simxGetObjectQuaternion(clientID,jointHandles[5],jointHandles[0],vrep.simx_opmode_blocking)
qx=theta_come2[0]
qy=theta_come2[1]
qz=theta_come2[2]
sleep(2)
errorCode, dummy_handle_3 = vrep.simxCreateDummy(
clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(clientID, dummy_handle_3, joint_handles[6],
(0, 0, 0), vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectOrientation(clientID, dummy_handle_3, joint_handles[6],
(0, 0, 0), vrep.simx_opmode_oneshot_wait)
errorCode, pos = vrep.simxGetObjectPosition(clientID, joint_handles[6],
joint_handles[0],
vrep.simx_opmode_oneshot_wait)
# print("Actual pos:", pos)
errorCode, angles = vrep.simxGetObjectQuaternion(
clientID, joint_handles[6], joint_handles[0],
vrep.simx_opmode_oneshot_wait)
print("Actual qua:", angles)
for i in range(7):
theta = vrep.simxGetJointPosition(clientID, joint_handles[i],
vrep.simx_opmode_oneshot_wait)
print("Theta", i, "is", theta)
# sleep(10)
input("Press any key to revert to origin...")
print("")
for i in range(7):
vrep.simxSetJointTargetPosition(clientID, joint_handles[i], 0,
vrep.simx_opmode_oneshot_wait)
def forward_k(joints, theta, clientID):
s = []
a = np.array([[0, 0, 1], [0, 1, 0], [0, 1, 0], [0, 1, 0], [0, 0, 1],
[0, 1, 0]])
x = 0
y = 0
z = 0
vector = [[0], [0], [0]]
for i in range(0, 6):
result, vector = vrep.simxGetObjectPosition(clientID, joints[i],
joints[0],
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get position')
q = np.array([[vector[0]], [vector[1]], [vector[2]]])
s.append(getS(a[i], q)[:, 0])
s = np.array(s).T
result, vector = vrep.simxGetObjectPosition(clientID, joint_end, joints[0],
vrep.simx_opmode_blocking)
result, theta_come = vrep.simxGetObjectQuaternion(
clientID, joint_end, joints[0], vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object orientation')
qx = theta_come[0]
qy = theta_come[1]
qz = theta_come[2]
qw = theta_come[3]
#M = [[np.cos(t2)*np.cos(t1), np.sin(t3)*np.sin(t2)*np.cos(t1)-np.cos(t3)*np.sin(t1),np.cos(t3)*np.sin(t2)*np.cos(t1)+np.sin(t3)*np.sin(t1),vector[0]],
#[np.cos(t2)*np.sin(t1),np.sin(t3)*np.sin(t2)*np.sin(t1)+np.cos(t3)*np.cos(t1),np.cos(t3)*np.sin(t2)*np.sin(t1)-np.sin(t3)*np.cos(t1),vector[1]],
#[-np.sin(t2),np.sin(t3)*np.cos(t2),np.cos(t3)*np.cos(t2),vector[2]],
M = np.array([[
1 - 2 * qy * qy - 2 * qz * qz, 2 * qx * qy - 2 * qz * qw,
2 * qx * qz + 2 * qy * qw, vector[0]
],
[
2 * qx * qy + 2 * qz * qw, 1 - 2 * qx * qx - 2 * qz * qz,
2 * qy * qz - 2 * qx * qw, vector[1]
],
[
2 * qx * qz - 2 * qy * qw, 2 * qy * qz + 2 * qx * qw,
1 - 2 * qx * qx - 2 * qy * qy, vector[2]
], [0, 0, 0, 1]])
T_test = exm(s[:, 0], theta[0]).dot(exm(s[:, 1], theta[1]))
T_test = T_test.dot(exm(s[:, 2], theta[2]))
T_test = T_test.dot(exm(s[:, 3], theta[3]))
T_test = T_test.dot(exm(s[:, 4], theta[4]))
T_test = T_test.dot(M)
T = exm(s[:, 0], theta[0]).dot(exm(s[:, 1], theta[1]))
T = T.dot(exm(s[:, 2], theta[2]))
T = T.dot(exm(s[:, 3], theta[3]))
T = T.dot(exm(s[:, 4], theta[4]))
T = T.dot(exm(s[:, 5], theta[5]))
T = T.dot(M)
return T
def obj_get_quaternion(self, handle, relative_to=None):
R = self.RAPI_rc(
vrep.simxGetObjectQuaternion(
self.cID, handle, -1 if relative_to is None else relative_to,
vrep.simx_opmode_blocking))
return R
try:
while True:
errorCode, pos = vrep.simxGetObjectPosition(
clientID, joint_handles[6], -1, vrep.simx_opmode_oneshot_wait)
#print("Initial end effector position:", pos)
# input("Press any key to move robot to desired location")
# Get the desired pose by checking the pose of the user-movable dummy
errorCode, dummy_pos = vrep.simxGetObjectPosition(
clientID, movable_dummy_handle, -1,
vrep.simx_opmode_oneshot_wait)
#print("Dummy pos: {}".format(dummy_pos))
errorCode, dummy_quaternion = vrep.simxGetObjectQuaternion(
clientID, movable_dummy_handle, -1,
vrep.simx_opmode_oneshot_wait)
#print("Dummy qua: {}".format(dummy_quaternion))
# Make sure we don't recalculate if the inverse kinematics dummy is still in the same position
if (dummy_pos == prev_dummy_pos):
continue
prev_dummy_pos = dummy_pos
# Mush the quaternion and pos into a pose matrix!
# Our function takes care of the V-REP quaternion mismatch
pose = quaternion.matrix_from_quaternion(dummy_quaternion)
pose[0, 3] = dummy_pos[0]
pose[1, 3] = dummy_pos[1]
pose[2, 3] = dummy_pos[2]
print("Pose is: \n{}".format(pose))
def get_obj_quat(handle, obj_name):
result, value = vrep.simxGetObjectQuaternion(clientID, handle, -1,
vrep.simx_opmode_oneshot_wait)
check_error(result, obj_name, 1)
return value
def get_object_quaternion(self, objectHandle):
retCode, quat = vrep.simxGetObjectQuaternion(self.client_id,
objectHandle, -1,
vrep.simx_opmode_blocking)
return quat
def forward_k(joints, theta, clientID):
s = []
a = np.array([[0, 0, 1], [0, 1, 0], [0, 1, 0], [0, 1, 0], [0, 0, 1],
[0, 1, 0]])
x = 0
y = 0
z = 0
vector = [[0], [0], [0]]
for i in range(0, 6):
result, vector = vrep.simxGetObjectPosition(clientID, joints[i],
joints[0],
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get position')
q = np.array([[vector[0]], [vector[1]], [vector[2]]])
s.append(getS(a[i], q)[:, 0])
s = np.array(s).T
#######################################################################################################
################################# get positions for all joints ########################################
#######################################################################################################
result, vector = vrep.simxGetObjectPosition(clientID, joint_end, joints[0],
vrep.simx_opmode_blocking)
result, theta_come = vrep.simxGetObjectQuaternion(
clientID, joint_end, joints[0], vrep.simx_opmode_blocking)
######################get initial Position and orientation for the end joint###########################
if result != vrep.simx_return_ok:
raise Exception('could not get object orientation')
qx = theta_come[0]
qy = theta_come[1]
qz = theta_come[2]
qw = theta_come[3]
M = np.array([[
1 - 2 * qy * qy - 2 * qz * qz, 2 * qx * qy - 2 * qz * qw,
2 * qx * qz + 2 * qy * qw, vector[0]
],
[
2 * qx * qy + 2 * qz * qw, 1 - 2 * qx * qx - 2 * qz * qz,
2 * qy * qz - 2 * qx * qw, vector[1]
],
[
2 * qx * qz - 2 * qy * qw, 2 * qy * qz + 2 * qx * qw,
1 - 2 * qx * qx - 2 * qy * qy, vector[2]
], [0, 0, 0, 1]])
# T = exm(s[:,0],theta[0]).dot(exm(s[:,1],theta[1]))
# T = T.dot(exm(s[:,2],theta[2]))
# T = T.dot(exm(s[:,3],theta[3]))
# T = T.dot(exm(s[:,4],theta[4]))
# T = T.dot(exm(s[:,5],theta[5]))
# T = T.dot(M)
#######################################################################################################
################## calculate theoretical pose for a set of joint variables ############################
#######################################################################################################
return s, M
# Agregamos el estado de detección al vector estadosDeteccion
estadosDeteccion.append(state)
# Agregamos el punto normalizado al vector puntosDetectados
puntosDetectados.append(np.linalg.norm(point))
"""
Creación del mapa
"""
xr, yr = worldToGrid(robotPosition[0], robotPosition[1], N, s)
print('Rows {} Cols {}'.format(yr, xr))
if xr >= N:
xr = N
if yr >= N:
yr = N
# Marcamos el espacio como vacio
matrizDeOcupacion[yr-1, xr-1] = 0
ret, srot = vrep.simxGetObjectQuaternion(clientID, sensorUltrasonico[i], -1, vrep.simx_opmode_blocking)
ret, spos = vrep.simxGetObjectPosition(clientID, sensorUltrasonico[i], -1, vrep.simx_opmode_blocking)
R = q2R(srot[0], srot[1], srot[2], srot[3])
spos = np.array(spos).reshape((3,1))
# if i % 2 != 0:
# continue
if state == True:
opos = np.array(point).reshape((3,1))
pobs = np.matmul(R, opos) + spos
xo, yo = worldToGrid(pobs[0], pobs[1], N, s)
if xo >= N:
xo = N
if yo >= N:
yo = N
rows, cols = line(yr-1, xr-1, yo-1, xo-1)
matrizDeOcupacion[rows, cols] = 0
'Pioneer_p3dx_ultrasonicSensor' + str(i),
vrep.simx_opmode_blocking)
usensor.append(s)
# Sensor initialization
for i in range(16):
err, state, point, detectedObj, detectedSurfNormVec = vrep.simxReadProximitySensor(
clientID, usensor[i], vrep.simx_opmode_streaming)
err, psr = vrep.simxGetObjectPosition(clientID, usensor[2], robot,
vrep.simx_opmode_streaming)
err = 1
while err:
err, psr = vrep.simxGetObjectPosition(clientID, usensor[2], robot,
vrep.simx_opmode_buffer)
err, qsr = vrep.simxGetObjectQuaternion(clientID, usensor[2], robot,
vrep.simx_opmode_streaming)
err = 1
while err:
err, qsr = vrep.simxGetObjectQuaternion(clientID, usensor[2], robot,
vrep.simx_opmode_buffer)
Rsr = q2R(qsr[0], qsr[1], qsr[2], qsr[3])
tsr = np.array(psr).reshape((3, 1))
t = time.time()
while (time.time() - t) < 10:
smeasure = []
sstate = []
for i in range(16):
err, state, point, detectedObj, detectedSurfNormVec = vrep.simxReadProximitySensor(
errh = angd - carrot[2]
uread = []
ustate = []
upt = []
for i in range(16):
err, state, point, detectedObj, detectedSurfNormVec = vrep.simxReadProximitySensor(
clientID, usensor[i], vrep.simx_opmode_buffer)
print(detectedObj)
ret, objpos = vrep.simxGetObjectPosition(clientID, detectedObj, -1,
vrep.simx_opmode_blocking)
print(objpos)
uread.append(np.linalg.norm(point))
upt.append(point)
ustate.append(state)
ret, srot = vrep.simxGetObjectQuaternion(clientID, usensor[i], -1,
vrep.simx_opmode_blocking)
ret, spos = vrep.simxGetObjectPosition(clientID, usensor[i], -1,
vrep.simx_opmode_blocking)
R = q2R(srot[0], srot[1], srot[2], srot[3])
spos = np.array(spos).reshape((3, 1))
if i % 2 != 0:
continue
if state == True:
opos = np.array(point).reshape((3, 1))
pobs = np.matmul(R, opos) + spos
xs = pobs[0]
ys = pobs[1]
xo = 50 + m.ceil(xs / 0.1)
yo = 50 - m.floor(ys / 0.1)
# alpha=np.array([[m1/n],[m2/n],[m3/n]])
# theta_forq=sla.norm(a_pre)
# w_new=np.cos(theta_forq/2)
# q1_new=alpha[0]*np.sin(theta_forq/2)
# q2_new=alpha[1]*np.sin(theta_forq/2)
# q3_new=alpha[2]*np.sin(theta_forq/2)
# new_orientation=[q1_new,q2_new,q3_new,w_new]
#result,frame=vrep.simxGetObjectHandle(clientID, 'ReferenceFrame', vrep.simx_opmode_blocking)
result, destination_position = vrep.simxGetObjectPosition(
clientID, frameHandles[i], jointHandles[0], vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, joint_smoke, jointHandles[0],
destination_position, vrep.simx_opmode_blocking)
result, destination_orientation = vrep.simxGetObjectQuaternion(
clientID, frameHandles[i], jointHandles[0], vrep.simx_opmode_blocking)
#######################################################################################################
#################### Now calculate the correspoding set of joint variables ############################
#######################################################################################################
qx = destination_orientation[0]
qy = destination_orientation[1]
qz = destination_orientation[2]
qw = destination_orientation[3]
M_destination = np.array([[
1 - 2 * qy * qy - 2 * qz * qz, 2 * qx * qy - 2 * qz * qw,
2 * qx * qz + 2 * qy * qw, destination_position[0]
],
[
