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] ], [