def turn_right(self, speed, angle):
rad = sim.simxGetObjectOrientation(self.simulation,
self.lf_motor_handle,
self.rf_wall_handle,
sim.simx_opmode_blocking)
old = rad[1][1] * (180 / math.pi)
obrocono = 0
while True:
if (obrocono >= angle):
break
self.setMotors(speed, -speed)
rad = sim.simxGetObjectOrientation(self.simulation,
self.lf_motor_handle,
self.rf_wall_handle,
sim.simx_opmode_blocking)
odczytana = rad[1][1] * (180 / math.pi)
monotonicznosc = old - odczytana
if (monotonicznosc <= 0):
obrocono += odczytana - old
else:
obrocono -= odczytana - old
old = odczytana
self.setMotors(0, 0)
def grasp_test(self):
# determine target position
targetID, targetPos, targetRot = self.determine_target('target')
res, targetPos = sim.simxGetObjectPosition(self.clientID, targetID,
self.worldCoord,
sim.simx_opmode_blocking)
res, targetRot = sim.simxGetObjectOrientation(self.clientID, targetID,
self.worldCoord,
sim.simx_opmode_blocking)
# use IK w/o orientation to reach target
self.IK_w_rotation(targetPos, targetRot, num_iter=500)
# lift with the same orientation
targetPos[2] += 0.2
res, targetRot = sim.simxGetObjectOrientation(self.clientID,
self.endEffector,
self.worldCoord,
sim.simx_opmode_blocking)
self.IK_w_rotation(targetPos, targetRot, alpha=0.3, num_iter=1000)
targetPos[0] -= 0.3
targetPos[1] += 0.2
self.IK(targetPos, targetRot, alpha=0.3, num_iter=500)
targetPos[2] -= 0.2
res, targetRot = sim.simxGetObjectOrientation(self.clientID,
self.endEffector,
self.worldCoord,
sim.simx_opmode_blocking)
self.IK_w_rotation(targetPos, targetRot, alpha=0.1, num_iter=500)
def forward_kinematics_test(self):
self.get_forward_kinematic(self)
self.get_current_pose(self)
for i in range(100):
config = np.random.rand(6)
self.apply_forward_kinematics(config)
self.apply_control(self.control)
self.compare_pose()
time.sleep(1)
return
# taget position
targetID, targetPos, targetRot = self.determine_target('target')
res, targetPos = sim.simxGetObjectPosition(self.clientID, targetID,
self.worldCoord,
sim.simx_opmode_oneshot)
res, targetRot = sim.simxGetObjectOrientation(self.clientID, targetID,
self.worldCoord,
sim.simx_opmode_oneshot)
x, y, z = targetPos
rx, ry, rz = targetRot
target = np.array([[x, y, z, rx, ry, rz]])
# get endeffector
res, tran = sim.simxGetObjectPosition(self.clientID, self.endEffector,
self.worldCoord,
sim.simx_opmode_oneshot)
res, rot = sim.simxGetObjectOrientation(self.clientID,
self.endEffector,
self.worldCoord,
sim.simx_opmode_oneshot)
end_pos = np.array([tran, rot]).flatten()
def get_forward_kinematic(self):
fk_list = []
res, tran = sim.simxGetObjectPosition(self.clientID, self.joints[0],
-1, sim.simx_opmode_blocking)
res, rot = sim.simxGetObjectOrientation(self.clientID, self.joints[0],
-1, sim.simx_opmode_blocking)
fk_list.append(util.matrix_transform(np.array(rot), np.array(tran)))
for idx in range(1, 7):
# transformation to parent frame
res, tran = sim.simxGetObjectPosition(self.clientID,
self.joints[idx],
self.joints[idx - 1],
sim.simx_opmode_blocking)
res, rot = sim.simxGetObjectOrientation(self.clientID,
self.joints[idx],
self.joints[idx - 1],
sim.simx_opmode_blocking)
fk_list.append(util.matrix_transform(rot, np.array(tran)))
# endeffector
res, tran = sim.simxGetObjectPosition(
self.clientID, self.endEffector, self.joints[len(self.joints) - 1],
sim.simx_opmode_blocking)
res, rot = sim.simxGetObjectOrientation(
self.clientID, self.endEffector, self.joints[len(self.joints) - 1],
sim.simx_opmode_blocking)
fk_list.append(util.matrix_transform(rot, np.array(tran)))
self.fk = np.array(fk_list)
def compute_jacobian(self, targetPos, targetRot):
# target position in global frame
x, y, z = targetPos
rx, ry, rz = targetRot
target = np.array([[x, y, z, rx, ry, rz]])
# end effector position in global frame
res, endEffPos = sim.simxGetObjectPosition(self.clientID,
self.endEffector,
self.worldCoord,
sim.simx_opmode_blocking)
x, y, z = endEffPos
res, endEffRot = sim.simxGetObjectOrientation(self.clientID,
self.endEffector,
self.worldCoord,
sim.simx_opmode_blocking)
rx, ry, rz = endEffRot
endEffectorPos = np.array([[x, y, z, rx, ry, rz]])
error = target - endEffectorPos
# start computing jacobian
jacobian = []
joint_q = []
res, trans1 = sim.simxGetObjectPosition(
self.clientID, self.endEffector, self.worldCoord,
sim.simx_opmode_blocking) # end effector in world
for idx in range(len(self.joints)):
# compute local jacobian
local_rot_axis = self.rot_axis[idx]
# find the current joint configuration
res, local_config = sim.simxGetJointPosition(
self.clientID, self.joints[idx], sim.simx_opmode_blocking)
joint_q.append((local_config))
# find transform matrix
res, rot = sim.simxGetObjectOrientation(self.clientID,
self.joints[idx],
self.worldCoord,
sim.simx_opmode_blocking)
rot_mat = util.get_rotation_matrix(rot)
# rotation axis in world coordinate
local_rot_axis = rot_mat.dot(local_rot_axis).flatten()
# joint position in world
res, trans2 = sim.simxGetObjectPosition(self.clientID,
self.joints[idx],
self.worldCoord,
sim.simx_opmode_blocking)
# find the translation from local position to world position
zi = np.array(trans1) - np.array(trans2)
j1 = np.cross(local_rot_axis, zi)
local_jacobian = np.hstack([j1, local_rot_axis])
jacobian.append(local_jacobian)
jacobian = np.array(jacobian).T
joint_q = np.array([joint_q])
# INVERSE JACOBIAN
return jacobian, error, joint_q
def rotate(self, angle):
speed = 1
def normalize(x):
return (x + math.pi) % (2 * math.pi)
rfactor = 2 if angle > 0 else 1
lfactor = 2 if angle < 0 else 1
self.steer(lfactor, rfactor, speed)
err, res = sim.simxGetObjectOrientation(self.client, self.base, -1,
sim.simx_opmode_blocking)
prev_ang = normalize(res[2])
rot = 0.0
while True:
err, res = sim.simxGetObjectOrientation(self.client, self.base, -1,
sim.simx_opmode_blocking)
cur_ang = normalize(res[2])
da = cur_ang - prev_ang
# From 0 to 2pi
if da > math.pi:
da = (2 * math.pi - cur_ang) + prev_ang
# from 2pi to 0
elif da < -math.pi:
da = cur_ang + (2 * math.pi - prev_ang)
rot += abs(da)
prev_ang = cur_ang
if rot >= abs(angle):
self.stop()
break
def __init__(self, queue_dict, sync=True):
"""
this class loads coppeliasim and is responsible for communication between the sim and python...
api functions can be found https://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm
"""
# open queues
self.queues = queue_dict
# close all connection that are remaining
sim.simxFinish(-1)
self.clientID = -1
attempt_num = 0
while self.clientID == -1:
print("attempting to connect to VREP...")
self.clientID = sim.simxStart('127.0.0.1', 19999,
True, True, 5000, 5)
attempt_num += 1
if attempt_num >= 3:
print("could not connect to vrep")
return
print("successful connection!")
self.sync = sync
if self.sync:
# set the simulation to synchronise with api
sim.simxSynchronous(self.clientID, True)
# get coppeliasim object handles
self.motor = {'left': self.getHandle("Pioneer_p3dx_leftMotor"),
'right': self.getHandle("Pioneer_p3dx_rightMotor")}
self.camHandle = self.getHandle("camera")
#self.gripperHandle = {"left": self.getHandle("left_joint"),
# "right": self.getHandle("right_joint")}
# init camera data stream with rgb and depth
sim.simxGetVisionSensorImage(self.clientID, self.camHandle, 0, sim.simx_opmode_streaming)
sim.simxGetVisionSensorDepthBuffer(self.clientID, self.camHandle, sim.simx_opmode_streaming)
# init position data stream with cartesian position and euler orientation
sim.simxGetObjectOrientation(self.clientID, self.camHandle, -1, sim.simx_opmode_streaming)
sim.simxGetObjectPosition(self.clientID, self.camHandle, -1, sim.simx_opmode_streaming)
# setting motor speed
self.speed = 1
self.lr = 0.3
self.images = None
# remote start simulation if not already
sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
# used for making keyboard switches
self.keyboard_key = Clicker("c", activated=True)
self.keyboard_controlled = self.keyboard_key.activated
self.gripper_key = Clicker("g", activated=False)
self.gripper_activated = self.gripper_key.activated
def set_drone_orientation(angle):
err, Qang = vrep.simxGetObjectOrientation(clientID, QuadricopterT, -1,
vrep.simx_opmode_oneshot_wait)
i = Qang[2]
while abs(Qang[2] - angle) > radians(2):
err, Qang = vrep.simxGetObjectOrientation(
clientID, QuadricopterT, -1, vrep.simx_opmode_oneshot_wait)
i += copysign(radians(1), angle - Qang[2])
err = vrep.simxSetObjectOrientation(clientID, QuadricopterT, -1,
(0, 0, i),
vrep.simx_opmode_oneshot)
time.sleep(0.1)
time.sleep(1)
def get_true_bbox(self):
xmin, xmax, ymin, ymax, zmin, zmax = self.boundary_points
# rotation relative to the desk
res, rot = sim.simxGetObjectOrientation(self.clientID, self.handle,
self.parent,
sim.simx_opmode_blocking)
rotation_matrix = util.matrix_transform(rot, self.actual_pos)
bbox = np.array([[xmin, ymin, zmin, 1], [xmin, ymax, zmin, 1],
[xmax, ymin, zmin, 1], [xmax, ymax, zmin, 1],
[xmin, ymin, zmax, 1], [xmin, ymax, zmax, 1],
[xmax, ymin, zmax, 1], [xmax, ymax, zmax, 1]])
bbox = rotation_matrix.dot(bbox.T)
# get the bounding boxes
xmax, ymax, zmax = np.max(bbox, axis=1)[0:3]
xmin, ymin, zmin = np.min(bbox, axis=1)[0:3]
self.bbox = np.array([[xmin, xmax],
[ymin, ymax]]) # bounding box w.r.t to the desk
# update change in dimension due to rotation
self.x_dim = xmax - xmin
self.y_dim = ymax - ymin
self.z_dim = zmax - zmin
return self.bbox
def scan(self):
#Method for getting the coordinates of a cloud of detected points in absolute/world reference
xy = []
for sonar in self.sonarHandles[0:16]: #For all sonars do
# Ping the sonar
res, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor(
clientID, sonar, sim.simx_opmode_buffer)
# If a point is detected:
if detectionState == 1:
# Get the sensor orientation in Euler Angles for the sensor
res, eulerAngle = sim.simxGetObjectOrientation(
clientID, sonar, -1, sim.simx_opmode_blocking)
# Get the position for the sonar vs the World frame (-1 value sees to that)
res, position = sim.simxGetObjectPosition(
clientID, sonar, -1, sim.simx_opmode_blocking)
# Rotate the coordinates of the found point to receive absolute coordinates (minus the offset of the sensor location)
v1 = rotate(detectedPoint[0], detectedPoint[1],
detectedPoint[2], eulerAngle[0], eulerAngle[1],
eulerAngle[2])
# Append the x and y coordinates of the detected poing plus the location of the sensor to get absolute coordinates
xy.append((position[0] + v1[0], position[1] + v1[1]))
return xy
def get_orientation(self) -> EulerAngles:
"""
Retrieves the linear and angular velocity.
@rtype EulerAngles
"""
code, orient = sim.simxGetObjectOrientation(self._id, self._handle, -1, self._def_op_mode)
return EulerAngles(orient[0], orient[1], orient[2])
def inicio_virar_SUL(): # para a função COM VISÃO
d=0
while(True):
erro,b=sim.simxGetObjectOrientation(glob.clientID,glob.robo,-1,sim.simx_opmode_blocking)
gamma=(b[2]*180)/(np.pi)
if(gamma>=-3 and gamma<=3):
Stop()
break
if(d==0):
if(gamma>0 and gamma<179.99):
d = 1
else:
d = -1
v=4
sim.simxPauseCommunication(glob.clientID, True)
sim.simxSetJointTargetVelocity(glob.clientID,glob.robotRightMotor,d*v, sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(glob.clientID,glob.robotLeftMotor,(-1)*d*v, sim.simx_opmode_oneshot)
sim.simxPauseCommunication(glob.clientID, False)
align.Align()
andar_em_metros(tras, 5, 0.065)
return
def normal_gamma(self):
"""Get rotation about z as a positive number"""
res, orientation = sim.simxGetObjectOrientation(
self.client_id, self.frame, -1, sim.simx_opmode_blocking)
orientation = orientation[2]
if orientation < 0: orientation = orientation + 2 * pi
return orientation
def inicio_virar_NORTE(): # para a função SEM VISÃO; lembrar de adicionar no if(nao viu prataleira) virar 180.
d=0
while(True):
erro,b=sim.simxGetObjectOrientation(glob.clientID,glob.robo,-1,sim.simx_opmode_blocking)
gamma=(b[2]*180)/(np.pi)
if(gamma>=177 or gamma<=-177):
Stop()
break
if(d==0):
if(gamma>0 and gamma<179.99):
d = 1
else:
d =-1
v=4
sim.simxPauseCommunication(glob.clientID, True)
sim.simxSetJointTargetVelocity(glob.clientID,glob.robotRightMotor,d*v, sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(glob.clientID,glob.robotLeftMotor,(-1)*d*v, sim.simx_opmode_oneshot)
sim.simxPauseCommunication(glob.clientID, False)
align.Align()
return
def __init__(self, clientID):
self.clientID = clientID
self.pioneer3DX_array = [None] * 19
self.position_coordX = [None] * 3
self.position_coordXc = [None] * 3
self.orientation = None
self.velocity = [None]* 2
self.ultrasonic = np.zeros((16, 3))
self.name = "Pioneer_p3dx"
self.left_motor = 'Pioneer_p3dx_leftMotor'
self.right_motor = 'Pioneer_p3dx_rightMotor'
self.ultrasonic_sensors = "Pioneer_p3dx_ultrasonicSensor"
### Load Mobile Robot Pioneer parameters
# self.pioneer3DX_array[0] represents the entire mobile robot block
error,self.pioneer3DX_array[0] = sim.simxGetObjectHandle(self.clientID,self.name,sim.simx_opmode_blocking)
# self.pioneer3DX_array[1] represents only the left motor
error,self.pioneer3DX_array[1] = sim.simxGetObjectHandle(self.clientID,self.left_motor,sim.simx_opmode_blocking)
# self.pioneer3DX_array[2] represents only the right motor
error,self.pioneer3DX_array[2] = sim.simxGetObjectHandle(self.clientID,self.right_motor,sim.simx_opmode_blocking)
# self.pioneer3DX_array[3:18] represents the 16 ultrasonic sensors
num = 1
while num < 17:
error,self.pioneer3DX_array[2+num] = sim.simxGetObjectHandle(self.clientID,self.ultrasonic_sensors+str(num),sim.simx_opmode_blocking)
error, DetectionState, Points ,detectedObjectHandle, Vector = sim.simxReadProximitySensor(self.clientID,self.pioneer3DX_array[2+num],sim.simx_opmode_streaming)
num+=1
error, linearVelocity, angularVelocity = sim.simxGetObjectVelocity(self.clientID,self.pioneer3DX_array[0], sim.simx_opmode_streaming)
error, position = sim.simxGetObjectPosition(self.clientID,self.pioneer3DX_array[0],-1, sim.simx_opmode_streaming)
error, angle = sim.simxGetObjectOrientation(self.clientID,self.pioneer3DX_array[0],-1,sim.simx_opmode_streaming)
print("Pioneer 3DX loaded")
def get_joint_data(clientID):
joints = {}
_, ids, _, _, names = sim.simxGetObjectGroupData(
clientID, simConst.sim_object_joint_type, 0, opmode)
for i in range(len(names)):
joints[names[i]] = sim.simxGetObjectOrientation(
clientID, ids[i], -1, opmode)[1]
return joints
def get_object_orientation(self, object_name):
# Get Object orientation in the world frame
err_code, object_h = sim.simxGetObjectHandle(self.client_id,
object_name,
sim.simx_opmode_blocking)
res, orientation = sim.simxGetObjectOrientation(
self.client_id, object_h, -1, sim.simx_opmode_blocking)
return array(orientation)
def change_robot_orientation(self):
ori_body = sim.simxGetObjectOrientation(self.clientID, self.khepera,
-1, sim.simx_opmode_buffer)
angles = ori_body[1][:]
angles[2] = 2 * np.pi * random.random()
errorCode = sim.simxSetObjectOrientation(self.clientID, self.khepera,
-1, angles,
sim.simx_opmode_oneshot)
def get_orientation(self, relative_object=-1):
# Get orientation relative to an object, -1 for global frame
if relative_object != -1:
relative_object = self._get_handler(relative_object)
res, euler = sim.simxGetObjectOrientation(self.client_id, self.frame,
relative_object,
sim.simx_opmode_oneshot)
return np.array(euler)
def ResetSimulationScene(self):
errorCode, resolution, image = sim.simxGetVisionSensorImage(
self.clientID, self.camera, 0, sim.simx_opmode_streaming)
pos = sim.simxGetObjectPosition(self.clientID, self.khepera, -1,
sim.simx_opmode_streaming)
ori_body = sim.simxGetObjectOrientation(self.clientID, self.khepera,
-1, sim.simx_opmode_streaming)
tar = sim.simxGetObjectPosition(self.clientID, self.target, -1,
sim.simx_opmode_streaming)
def get_orientation(self, relative_object=-1):
# Get orientation relative to an object, -1 for global frame
if relative_object != -1:
relative_object = self._get_handler(relative_object)
res, position = sim.simxGetObjectOrientation(self.client_id,
self.frame,
relative_object,
sim.simx_opmode_blocking)
return array(position)[2]
def turn_straight_right(self, speed):
old = sim.simxGetObjectOrientation(self.simulation,
self.lf_motor_handle,
self.rf_wall_handle,
sim.simx_opmode_blocking)
while True:
self.setMotors(speed, -speed)
new = sim.simxGetObjectOrientation(self.simulation,
self.lf_motor_handle,
self.rf_wall_handle,
sim.simx_opmode_blocking)
if abs(old[1][1] - new[1][1]) > 1:
if (new[1][1] > 1.52):
break
elif (new[1][1] < -1.52):
break
elif (new[1][1] < 0.05 and new[1][1] > -0.05):
break
self.setMotors(0, 0)
def get_PositionData(self):
## Pioneer Velocity
error, linearVelocity, angularVelocity = sim.simxGetObjectVelocity(self.clientID,self.pioneer3DX_array[0], sim.simx_opmode_buffer)
## Pioneer Position
error, self.position_coordXc = sim.simxGetObjectPosition(self.clientID,self.pioneer3DX_array[0],-1, sim.simx_opmode_buffer)
## Pioneer Orientation (alpha, beta e gama)
error, angle = sim.simxGetObjectOrientation(self.clientID,self.pioneer3DX_array[0],-1,sim.simx_opmode_buffer)
self.orientation = angle[2]
# Linear Transform to find the Control Point
A = np.array([np.cos(angle[2]), np.sin(angle[2]), 0])
self.position_coordX = self.position_coordXc + 0.15*A
def getPositionRobot(self):
pos = sim.simxGetObjectPosition(self.clientID, self.khepera, -1,
sim.simx_opmode_buffer)
ori_body = sim.simxGetObjectOrientation(self.clientID, self.khepera,
-1, sim.simx_opmode_buffer)
theta = ori_body[1][2]
if theta < 0:
theta = theta + 2.0 * np.pi
xc = pos[1][0]
yc = pos[1][1]
return xc, yc, theta
def rotates_z(handle, deg):
rotates(handle, deg)
arc = cal_arc(45)
res, ori0 = vrep.simxGetObjectOrientation(clientID, v0, -1,
vrep.simx_opmode_oneshot_wait)
res = vrep.simxSetObjectOrientation(clientID, handle, -1,
(ori0[0], ori0[1], ori0[2] + arc),
vrep.simx_opmode_oneshot_wait)
rotates(handle, deg)
res = vrep.simxSetObjectOrientation(clientID, handle, -1,
(ori0[0], ori0[1], ori0[2]),
vrep.simx_opmode_oneshot_wait)
def determine_target(self, target):
# init helper
res, target = sim.simxGetObjectHandle(self.clientID, target,
sim.simx_opmode_blocking)
init_helper(self.clientID, target, self.worldCoord)
res, targetPos = sim.simxGetObjectPosition(self.clientID, target,
self.worldCoord,
sim.simx_opmode_blocking)
res, targetRot = sim.simxGetObjectOrientation(self.clientID, target,
self.worldCoord,
sim.simx_opmode_blocking)
return target, targetPos, targetRot
def getGeographicalDirection(self):
new = sim.simxGetObjectOrientation(self.simulation,
self.lf_motor_handle,
self.rf_wall_handle,
sim.simx_opmode_blocking)
if (new[1][1] > 1 or new[1][1] < -1):
# north/south
return 0
else:
# west/east
return 1
def get_ori_pos(obj, rel):
res, rel_handle = vrep.simxGetObjectHandle(clientID, rel,
vrep.simx_opmode_oneshot_wait)
res, obj_handle = vrep.simxGetObjectHandle(clientID, obj,
vrep.simx_opmode_oneshot_wait)
res, pos = vrep.simxGetObjectPosition(clientID, obj_handle, rel_handle,
vrep.simx_opmode_streaming)
res, ori = vrep.simxGetObjectOrientation(clientID, obj_handle, rel_handle,
vrep.simx_opmode_streaming)
res, quat = vrep.simxGetObjectQuaternion(clientID, obj_handle, rel_handle,
vrep.simx_opmode_streaming)
return pos, quat
def main_loop(self):
"""
this is looped over in the sub process
"""
if self.sync:
sim.simxSynchronousTrigger(self.clientID)
sim.simxPauseCommunication(self.clientID, True)
# get position data
_, orientation = sim.simxGetObjectOrientation(self.clientID,
self.camHandle,
-1,
sim.simx_opmode_buffer)
_, position = sim.simxGetObjectPosition(self.clientID,
self.camHandle,
-1,
sim.simx_opmode_buffer)
# retrieve images
self.images = self.getImages()
# only send or receive once images are not none
if self.images is None:
time.sleep(1 / 100)
return
# send images to depth pipeline
self.images["display"] = self.Depth2Color(self.images["depth"], 1)
self.images["display"] = np.vstack([self.images["RGB"], self.images["display"]])
message = {"position": position, "orientation": orientation, "images": self.images}
self.queues["output"].send(message, method="recent")
speeds = self.queues["input"].retrieve(method="no_wait")
# check keyboard to set wheel directions
speed_L, speed_R = self.keyboardInput()
if (speeds is not None) and (not self.keyboard_controlled):
speed_L = speeds["left"]
speed_R = speeds["right"]
# activate gripper
#if self.gripper_key.click():
# self.setGripper("left", -0.05)
# self.setGripper("right", 0.05)
#else:
# self.setGripper("left", 0.05)
# self.setGripper("right", -0.05)
# set wheel speeds
threads = {"left": Thread(target=self.setSpeed, args=("left", speed_L,)),
"right": Thread(target=self.setSpeed, args=("right", speed_R,))}
self.startThreads(threads)
self.joinThreads(threads)
sim.simxPauseCommunication(self.clientID, False)
def sensing(self):
res, self.imuPos = sim.simxGetObjectPosition(self.id, self.imu, -1,
sim.simx_opmode_blocking)
res, self.imuOrient = sim.simxGetObjectOrientation(
self.id, self.imu, -1, sim.simx_opmode_blocking)
res, self.baro_value = sim.simxGetObjectPosition(
self.id, self.baro, -1, sim.simx_opmode_blocking)
res, self.gps_value = sim.simxGetObjectOrientation(
self.id, self.gps, -1, sim.simx_opmode_blocking)
res, self.gps_pos = sim.simxGetObjectPosition(self.id, self.gps, -1,
sim.simx_opmode_blocking)
# Get relevant values
self.altitude = self.baro_value[2]
self.compass = self.gps_value[2]
self.gpsPos[0] = self.gps_pos[0]
self.gpsPos[1] = self.gps_pos[1]
# Calculate velocity and acceleration, linear + angular
for i in range(3):
self.linearVel[i] = self.imuPos[i] - self.imuPos_old[i]
self.angularVel[i] = self.imuOrient[i] - self.imuOrient_old[i]
self.linearAcc[i] = self.linearVel[i] - self.linearVel_old[i]
self.angularAcc[i] = self.angularVel[i] - self.angularVel_old[i]
# Center position and orientation
self.position[0] = (self.imuPos[0] + self.gpsPos[0]) * 0.5
self.position[1] = (self.imuPos[1] + self.gpsPos[1]) * 0.5
self.position[2] = (self.imuPos[2] + self.altitude) * 0.5
self.orientation[0] = self.imuOrient[0]
self.orientation[1] = self.imuOrient[2]
self.orientation[2] = (self.imuOrient[2] + self.compass) * 0.5
# Update
self.imuPos_old = self.imuPos
self.imuOrient_old = self.imuOrient
self.linearVel_old = self.linearVel
self.angularVel_old = self.angularVel
