def getExtendedObservation(self): self._observation = self._kuka.getObservation() gripperState = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaGripperIndex) gripperPos = gripperState[0] gripperOrn = gripperState[1] blockPos,blockOrn = p.getBasePositionAndOrientation(self.blockUid) invGripperPos,invGripperOrn = p.invertTransform(gripperPos,gripperOrn) gripperMat = p.getMatrixFromQuaternion(gripperOrn) dir0 = [gripperMat[0],gripperMat[3],gripperMat[6]] dir1 = [gripperMat[1],gripperMat[4],gripperMat[7]] dir2 = [gripperMat[2],gripperMat[5],gripperMat[8]] gripperEul = p.getEulerFromQuaternion(gripperOrn) #print("gripperEul") #print(gripperEul) blockPosInGripper,blockOrnInGripper = p.multiplyTransforms(invGripperPos,invGripperOrn,blockPos,blockOrn) projectedBlockPos2D =[blockPosInGripper[0],blockPosInGripper[1]] blockEulerInGripper = p.getEulerFromQuaternion(blockOrnInGripper) #print("projectedBlockPos2D") #print(projectedBlockPos2D) #print("blockEulerInGripper") #print(blockEulerInGripper) #we return the relative x,y position and euler angle of block in gripper space blockInGripperPosXYEulZ =[blockPosInGripper[0],blockPosInGripper[1],blockEulerInGripper[2]] #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir0[0],gripperPos[1]+dir0[1],gripperPos[2]+dir0[2]],[1,0,0],lifeTime=1) #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir1[0],gripperPos[1]+dir1[1],gripperPos[2]+dir1[2]],[0,1,0],lifeTime=1) #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir2[0],gripperPos[1]+dir2[1],gripperPos[2]+dir2[2]],[0,0,1],lifeTime=1) self._observation.extend(list(blockInGripperPosXYEulZ)) return self._observation
def _reward(self): current_base_position = self.minitaur.GetBasePosition() forward_reward = current_base_position[0] - self._last_base_position[0] # Cap the forward reward if a cap is set. forward_reward = min(forward_reward, self._forward_reward_cap) # Penalty for sideways translation. drift_reward = -abs(current_base_position[1] - self._last_base_position[1]) # Penalty for sideways rotation of the body. orientation = self.minitaur.GetBaseOrientation() rot_matrix = pybullet.getMatrixFromQuaternion(orientation) local_up_vec = rot_matrix[6:] shake_reward = -abs(np.dot(np.asarray([1, 1, 0]), np.asarray(local_up_vec))) energy_reward = -np.abs( np.dot(self.minitaur.GetMotorTorques(), self.minitaur.GetMotorVelocities())) * self._time_step objectives = [forward_reward, energy_reward, drift_reward, shake_reward] weighted_objectives = [o * w for o, w in zip(objectives, self._objective_weights)] reward = sum(weighted_objectives) self._objectives.append(objectives) return reward
def getExtendedObservation(self): self._observation = self._kuka.getObservation() gripperState = p.getLinkState(self._kuka.kukaUid, self._kuka.kukaGripperIndex) gripperPos = gripperState[0] gripperOrn = gripperState[1] blockPos, blockOrn = p.getBasePositionAndOrientation(self.blockUid) invGripperPos, invGripperOrn = p.invertTransform( gripperPos, gripperOrn) gripperMat = p.getMatrixFromQuaternion(gripperOrn) dir0 = [gripperMat[0], gripperMat[3], gripperMat[6]] dir1 = [gripperMat[1], gripperMat[4], gripperMat[7]] dir2 = [gripperMat[2], gripperMat[5], gripperMat[8]] gripperEul = p.getEulerFromQuaternion(gripperOrn) #print("gripperEul") #print(gripperEul) blockPosInGripper, blockOrnInGripper = p.multiplyTransforms( invGripperPos, invGripperOrn, blockPos, blockOrn) projectedBlockPos2D = [blockPosInGripper[0], blockPosInGripper[1]] blockEulerInGripper = p.getEulerFromQuaternion(blockOrnInGripper) #print("projectedBlockPos2D") #print(projectedBlockPos2D) #print("blockEulerInGripper") #print(blockEulerInGripper) #we return the relative x,y position and euler angle of block in gripper space blockInGripperPosXYEulZ = [ blockPosInGripper[0], blockPosInGripper[1], blockEulerInGripper[2] ] #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir0[0],gripperPos[1]+dir0[1],gripperPos[2]+dir0[2]],[1,0,0],lifeTime=1) #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir1[0],gripperPos[1]+dir1[1],gripperPos[2]+dir1[2]],[0,1,0],lifeTime=1) #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir2[0],gripperPos[1]+dir2[1],gripperPos[2]+dir2[2]],[0,0,1],lifeTime=1) self._observation.extend(list(blockInGripperPosXYEulZ)) return self._observation
def get_trimesh_scene(axis: bool = False, bbox: bool = False) -> trimesh.Scene: """Returns trimesh scene.""" import pybullet scene = trimesh.Scene() for unique_id in unique_ids: _, _, shape_id, _, mesh_file, *_ = pybullet.getVisualShapeData( unique_id )[0] mesh_file = mesh_file.decode() if pybullet.GEOM_MESH != shape_id: raise ValueError( f"Unsupported shape_id: {shape_id_to_str(shape_id)}" ) pos, ori = pybullet.getBasePositionAndOrientation(unique_id) t = np.array(pos, dtype=float) R = pybullet.getMatrixFromQuaternion(ori) R = np.array(R, dtype=float).reshape(3, 3) transform = geometry.compose_transform(R=R, t=t) mesh = trimesh.load_mesh(mesh_file) scene.add_geometry( mesh, node_name=str(unique_id), transform=transform, ) if bbox: scene.add_geometry( trimesh.path.creation.box_outline(mesh.bounding_box), transform=transform, ) if axis: origin_size = np.max(mesh.bounding_box.extents) * 0.05 scene.add_geometry( trimesh.creation.axis(origin_size), transform=transform, ) return scene
def _imu(self): p = self._p [quart_link, lin_vel, ang_vel] = p.getLinkState(bodyUniqueId=self.soccerbotUid, linkIndex=Links.IMU, computeLinkVelocity=1)[5:8] lin_vel = np.array(lin_vel, dtype=self.DTYPE) lin_acc = (lin_vel - self.prev_lin_vel) / self.timeStep lin_acc -= self._GRAVITY_VECTOR rot_mat = np.array(pb.getMatrixFromQuaternion(quart_link), dtype=self.DTYPE).reshape((3, 3)) lin_acc = np.matmul(rot_mat, lin_acc) ang_vel = np.array(ang_vel, dtype=self.DTYPE) self.prev_lin_vel = lin_vel imu_noise = np.concatenate( (self.np_random.normal(0, self._IMU_LIN_STDDEV, int(self._IMU_DIM / 2)), self.np_random.normal(0, self._IMU_ANG_STDDEV, int(self._IMU_DIM / 2)))) imu_val = np.concatenate( (lin_acc, ang_vel)) + imu_noise + self.imu_bias imu_val = np.clip(imu_val, -self.imu_limit, self.imu_limit) return imu_val
def reconstruct_heightmaps(color, depth, configs, bounds, pixel_size): """Reconstruct top-down heightmap views from multiple 3D pointclouds. The color and depth are np.arrays or lists, where the leading dimension or list wraps around differnet viewpoints. So, if the np.array shape is (3,480,640,3), then the leading '3' denotes the number of camera views. TODO: documentation. """ heightmaps, colormaps = [], [] for color, depth, config in zip(color, depth, configs): intrinsics = np.array(config['intrinsics']).reshape(3, 3) xyz = get_pointcloud(depth, intrinsics) position = np.array(config['position']).reshape(3, 1) rotation = p.getMatrixFromQuaternion(config['rotation']) rotation = np.array(rotation).reshape(3, 3) transform = np.eye(4) transform[:3, :] = np.hstack((rotation, position)) xyz = transform_pointcloud(xyz, transform) heightmap, colormap = get_heightmap(xyz, color, bounds, pixel_size) heightmaps.append(heightmap) colormaps.append(colormap) return heightmaps, colormaps
def addDebugFrame(pos, quat, length=0.2, lw=2, duration=0.2, pcid=0): rot_mat = np.array(p.getMatrixFromQuaternion(quat)).reshape(3, 3).transpose() toX = pos + length * rot_mat[0, :] toY = pos + length * rot_mat[1, :] toZ = pos + length * rot_mat[2, :] item_id = [0, 0, 0] item_id[0] = p.addUserDebugLine(pos, toX, [1, 0, 0], lw, duration, physicsClientId=pcid) item_id[1] = p.addUserDebugLine(pos, toY, [0, 1, 0], lw, duration, physicsClientId=pcid) item_id[2] = p.addUserDebugLine(pos, toZ, [0, 0, 1], lw, duration, physicsClientId=pcid) return item_id
def getExtendedObservation(self): carpos, carorn = self._p.getBasePositionAndOrientation( self._racecar.racecarUniqueId) print('RACE CAR ID:', self._racecar.racecarUniqueId, carorn) carmat = p.getMatrixFromQuaternion(carorn) ballpos, ballorn = self._p.getBasePositionAndOrientation( self._ballUniqueId) invCarPos, invCarOrn = self._p.invertTransform(carpos, carorn) ballPosInCar, ballOrnInCar = self._p.multiplyTransforms( invCarPos, invCarOrn, ballpos, ballorn) dist0 = 0.3 dist1 = 1. eyePos = [ carpos[0] + dist0 * carmat[0], carpos[1] + dist0 * carmat[3], carpos[2] + dist0 * carmat[6] + 0.3 ] targetPos = [ carpos[0] + dist1 * carmat[0], carpos[1] + dist1 * carmat[3], carpos[2] + dist1 * carmat[6] + 0.3 ] up = [carmat[2], carmat[5], carmat[8]] viewMat = self._p.computeViewMatrix(eyePos, targetPos, up) #viewMat = self._p.computeViewMatrixFromYawPitchRoll(carpos,1,0,0,0,2) #print("projectionMatrix:") #print(self._p.getDebugVisualizerCamera()[3]) projMatrix = [ 0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0 ] img_arr = self._p.getCameraImage(width=self._width, height=self._height, viewMatrix=viewMat, projectionMatrix=projMatrix) rgb = img_arr[2] np_img_arr = np.reshape(rgb, (self._height, self._width, 4)) self._observation = np_img_arr return self._observation
def ahead_view(self): link_state = pb.getBasePositionAndOrientation(self.agent_mb) link_p = link_state[0] link_o = link_state[1] #handmat = pb.getMatrixFromQuaternion(link_o) #axisX = [handmat[0],handmat[3],handmat[6]] #axisY = [-handmat[1],-handmat[4],-handmat[7]] # Negative Y axis #axisZ = [handmat[2],handmat[5],handmat[8]] # we define the up axis in terms of the object to classify since it is # stationary. If you define up in terms of the orientation of the wand # then your up axis can rotate as the wand rotates due to the normal # contact forces with the object to classify. This causes the camera # image to exhibit some unwanted behavior, such as inverting the up # and down directions. # If your object to classify is not stationary, then you could switch to # the above code to use the wand orientation to define your up axis. state = pb.getBasePositionAndOrientation(self.obj_to_classify) state_o = state[1] mat = pb.getMatrixFromQuaternion(state_o) axisZ2 = [mat[2], mat[5], mat[8]] eyeOffset = self.wandLength focusOffset = eyeOffset + 10e-6#0.0000001 self.eye_pos = [link_p[0] + eyeOffset, link_p[1], link_p[2]] self.target_pos = [link_p[0] + focusOffset, link_p[1], link_p[2]] up = axisZ2 # Up is Z axis of obj_to_classify - change to axisZ to use # the wand orientation as up viewMatrix = pb.computeViewMatrix(self.eye_pos, self.target_pos, up) if 'render' in self.options and self.options['render'] == True: pass return viewMatrix
def compute_p_W_and_n_W(self, phi_B): """ Given a planar angle in the object frame B, perform ray cast from the origin of the B frame and find the corresponding surface normal and ray impact position in the world frame W. FIXME(wualbert): We don't care about z as we are in 2D. This may cause problems later. @param phi_B: The yaw of the contact point in the B frame. @return: (p_W, n_W). p_W is the 3D contact point on the object. n_W is the 3D outward-facing unit normal vector at the contact point. Both of these are in the world frame. """ # Get the pose of the object frame origin in world frame p_B_W, q_B_W = p.getBasePositionAndOrientation(self.body_id) e_B_W = p.getEulerFromQuaternion(q_B_W) # In 2D, only the yaw should be nonzero np.testing.assert_allclose(e_B_W[:2], np.zeros(2), atol=1e-4) # add phi_B e_phi_W = e_B_W + np.asarray([0., 0., phi_B]) e_phi_W %= 2 * np.pi q_phi_W = p.getQuaternionFromEuler(e_phi_W) # Construct the unit ray vector # Compute the ray to cast ray_direction_W = np.dot( np.array(p.getMatrixFromQuaternion(q_phi_W)).reshape(3, 3), np.asarray([1., 0., 0.])) # FIXME(wualbert): the ray shouldn't be of arbitrary length ray_start_W = p_B_W + ray_direction_W * 10. ray_end_W = p_B_W - ray_direction_W * 10. ans = p.rayTest(ray_start_W, ray_end_W) for a in ans: if a[0] == self.body_id: # FIXME(wualbert): a cleaner way to do this # We only care about the normal of this particular object return a[3:] raise AssertionError
def get_camera_image(self): image = np.zeros((100, 100, 4)) proj_matrix = p.computeProjectionMatrixFOV(fov=80, aspect=1, nearVal=0.01, farVal=100) pos, ori = [ list(l) for l in p.getBasePositionAndOrientation(self.id, self.client) ] pos[2] = 0.2 # Rotate camera direction rot_mat = np.array(p.getMatrixFromQuaternion(ori)).reshape(3, 3) camera_vec = np.matmul(rot_mat, [1, 0, 0]) up_vec = np.matmul(rot_mat, np.array([0, 0, 1])) view_matrix = p.computeViewMatrix(pos, pos + camera_vec, up_vec) w, h, rgb, depth, self.segmask = p.getCameraImage( 100, 100, view_matrix, proj_matrix) rgb = rgb[:, :, :-1] / 255. rgbd = np.concatenate([rgb, np.expand_dims(depth, 2)], axis=2) return rgbd
def getCameraImage(self): """Return camera image from CAMERA_JOINT_INDEX. """ # compute eye and target position for viewMatrix pos, orn, _, _, _, _ = p.getLinkState(self.robotId, self.CAMERA_JOINT_INDEX) cameraPos = np.array(pos) cameraMat = np.array(p.getMatrixFromQuaternion(orn)).reshape(3, 3).T eyePos = cameraPos + self.CAMERA_EYE_SCALE * cameraMat[ self.CAMERA_EYE_INDEX] targetPos = cameraPos + self.CAMERA_TARGET_SCALE * cameraMat[ self.CAMERA_EYE_INDEX] up = self.CAMERA_UP_SCALE * cameraMat[self.CAMERA_UP_INDEX] # p.addUserDebugLine(eyePos, targetPos, lineColorRGB=[1, 0, 0], lifeTime=0.1) # red line for camera vector # p.addUserDebugLine(eyePos, eyePos + up * 0.5, lineColorRGB=[0, 0, 1], lifeTime=0.1) # blue line for up vector viewMatrix = p.computeViewMatrix(eyePos, targetPos, up) image = p.getCameraImage(self.CAMERA_PIXEL_WIDTH, self.CAMERA_PIXEL_HEIGHT, viewMatrix, self.projectionMatrix, shadow=1, lightDirection=[1, 1, 1], renderer=p.ER_BULLET_HARDWARE_OPENGL) return image
def _getObservation(self): allJoints = [j.value for j in Joints] jointStates = p.getJointStates(self.anymal, allJoints) position = np.array([js[0] for js in jointStates]) velocity = np.array([js[1] for js in jointStates]) # global lastVelocity # if self.t == 0.0: # lastVelocity = velocity if len(self.history_buffer['joint_state']['velocity']) == 0: acceleration = np.zeros(12) else: lastVelocity = self.history_buffer['joint_state']['velocity'][-1] acceleration = (velocity - lastVelocity) * SIMULATIONFREQUENCY # lastVelocity = velocity basePosition, baseOrientation = p.getBasePositionAndOrientation( self.anymal) baseOrientation_Matrix = np.array( p.getMatrixFromQuaternion(baseOrientation)).reshape(3, 3) baseOrientationVector = np.matmul(baseOrientation_Matrix, [0, 0, -1]) baseVelocity, baseAngularVelocity = p.getBaseVelocity(self.anymal) observationAsArray = np.concatenate([ position, velocity, basePosition, baseOrientation, baseOrientationVector, baseVelocity, baseAngularVelocity, acceleration ]) observationAsDict = { "position": position, "velocity": velocity, "basePosition": basePosition, "baseOrientation": baseOrientation, "baseOrientationVector": baseOrientationVector, "baseVelocity": baseVelocity, "baseAngularVelocity": baseAngularVelocity, "acceleration": acceleration } return observationAsArray, observationAsDict
def addDebugLinkFrame(self, linkId, axisLength=0.2, axisWidth=1): localTransCom = self.getLinkLocalInertialTransform(linkId) comPosLocal, comQuatLocal = pybullet.invertTransform( localTransCom[0], localTransCom[1]) comRotLocal = np.array( pybullet.getMatrixFromQuaternion(comQuatLocal)).reshape((3, 3)) pybullet.addUserDebugLine(comPosLocal, comPosLocal + comRotLocal[:, 0] * axisLength, Color.red, lineWidth=axisWidth, parentObjectUniqueId=self.id, parentLinkIndex=linkId) pybullet.addUserDebugLine(comPosLocal, comPosLocal + comRotLocal[:, 1] * axisLength, Color.green, lineWidth=axisWidth, parentObjectUniqueId=self.id, parentLinkIndex=linkId) pybullet.addUserDebugLine(comPosLocal, comPosLocal + comRotLocal[:, 2] * axisLength, Color.blue, lineWidth=axisWidth, parentObjectUniqueId=self.id, parentLinkIndex=linkId)
def get_imu_raw(self, verbose=False): """ Simulates the IMU at the IMU link location. TODO: Add noise model, make the refresh rate vary (currently in sync with the PyBullet time steps) :param verbose: Optional - Set to True to print the linear acceleration and angular velocity :return: concatenated 3-axes values for linear acceleration and angular velocity """ [quart_link, lin_vel, ang_vel] = pb.getLinkState(self.body, linkIndex=Links.IMU, computeLinkVelocity=1)[5:8] # [lin_vel, ang_vel] = p.getLinkState(bodyUniqueId=self.soccerbotUid, linkIndex=Links.HEAD_1, computeLinkVelocity=1)[6:8] # print(p.getLinkStates(bodyUniqueId=self.soccerbotUid, linkIndices=range(0,20,1), computeLinkVelocity=1)) # p.getBaseVelocity(self.soccerbotUid) lin_vel = np.array(lin_vel, dtype=np.float32) self.gravity = [0, 0, -9.81] lin_acc = (lin_vel - self.prev_lin_vel) / self.time_step_sim lin_acc -= self.gravity rot_mat = np.array(pb.getMatrixFromQuaternion(quart_link), dtype=np.float32).reshape((3, 3)) lin_acc = np.matmul(rot_mat, lin_acc) ang_vel = np.array(ang_vel, dtype=np.float32) self.prev_lin_vel = lin_vel if verbose: print(f'lin_acc = {lin_acc}', end="\t\t") print(f'ang_vel = {ang_vel}') return np.concatenate((lin_acc, ang_vel))
def render(self, mode='human'): if self.rendered_img is None: self.rendered_img = plt.imshow(np.zeros((100, 100, 4))) # Base information car_id, client_id = self.car.get_ids() proj_matrix = p.computeProjectionMatrixFOV(fov=80, aspect=1, nearVal=0.01, farVal=100) pos, ori = [list(l) for l in p.getBasePositionAndOrientation(car_id, client_id)] pos[2] = 0.2 # Rotate camera direction rot_mat = np.array(p.getMatrixFromQuaternion(ori)).reshape(3, 3) camera_vec = np.matmul(rot_mat, [1, 0, 0]) up_vec = np.matmul(rot_mat, np.array([0, 0, 1])) view_matrix = p.computeViewMatrix(pos, pos + camera_vec, up_vec) # Display image frame = p.getCameraImage(100, 100, view_matrix, proj_matrix)[2] frame = np.reshape(frame, (100, 100, 4)) self.rendered_img.set_data(frame) plt.draw() plt.pause(.00001)
def getViewData(self): com_p, com_o = pybullet.getBasePositionAndOrientation(self._demon) rot_matrix = pybullet.getMatrixFromQuaternion(com_o) rot_matrix = np.array(rot_matrix).reshape(3, 3) # Initial vectors # init_camera_vector = (0, 0, -1) # z-axis # init_up_vector = (0, 1, 0) # y-axis # Rotated vectors # camera_vector = rot_matrix.dot(init_camera_vector) # up_vector = rot_matrix.dot(init_up_vector) # view_matrix = p.computeViewMatrix(com_p, com_p + 0.1 * camera_vector, up_vector) view_matrix = pybullet.computeViewMatrix( cameraEyePosition=[0, 0, 15], cameraTargetPosition=[0, 0, 0], cameraUpVector=[0, 1, 0]) fov, aspect, nearplane, farplane = 60, 1.0, 0.01, 100 projection_matrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearplane, farplane) # img = p.getCameraImage(1000, 1000, view_matrix) (w,y,img,depth,segment) = pybullet.getCameraImage(width=self._render_shape[0], height=self._render_shape[1], viewMatrix=view_matrix, projectionMatrix=projection_matrix) # print (img) return img[..., :3]
def render(self): # Base information kuka_id, client_id = self.kuka_iiwa.get_ids() proj_matrix = p.computeProjectionMatrixFOV(fov=80, aspect=1, nearVal=0.01, farVal=100) pos, ori = [ list(l) for l in p.getBasePositionAndOrientation(kuka_id, client_id) ] # Rotate camera direction rot_mat = np.array(p.getMatrixFromQuaternion(ori)).reshape(3, 3) camera_vec = np.matmul(rot_mat, [1, 0, 0]) up_vec = [0, 0, 1] view_matrix = p.computeViewMatrix(pos, pos + camera_vec, up_vec) # Display image _, _, frame, _, _ = p.getCameraImage( 800, 800) #, view_matrix, proj_matrix)[2] frame = np.reshape(frame, (800, 800, 4)) self.rendered_img.set_data(frame) return frame
def get_robot_observation(self, with_vel=False): obs = [] # root z and root rot mat (1+9) root_pos, root_orn = self.get_link_com_xyz_orn(-1) root_x, root_y, root_z = root_pos obs.extend([root_z]) obs.extend(pybullet.getMatrixFromQuaternion(root_orn)) # root lin vel (3) base_vel, base_ang_vel = self._p.getBaseVelocity(self.go_id) obs.extend(base_vel) # non-root joint q (12) q, dq = self.get_q_dq(self.ctrl_dofs) obs.extend(q) # feet (offset from root) (12) for link in self.feet: pos, _ = self.get_link_com_xyz_orn(link, fk=1) pos[0] -= root_x pos[1] -= root_y pos[2] -= root_z obs.extend(pos) length_wo_vel = len(obs) obs.extend(base_ang_vel) # (3) obs.extend(dq) # (12) obs = np.array(obs) * self.robo_obs_scale if not with_vel: obs = obs[:length_wo_vel] return list(obs)
def pick(self, pos, rot, offset, dynamic=True, objects=None, simulate_grasp=True): '''''' # Setup pre-grasp pos and default orientation self.openGripper() pre_pos = copy.copy(pos) m = np.array(pb.getMatrixFromQuaternion(rot)).reshape(3, 3) pre_pos += m[:, 2] * offset # rot = pb.getQuaternionFromEuler([np.pi/2.,-np.pi,np.pi/2]) pre_rot = rot # Move to pre-grasp pose and then grasp pose self.moveTo(pre_pos, pre_rot, dynamic) if simulate_grasp: self.moveTo(pos, rot, True, pos_th=1e-3, rot_th=1e-3) # Grasp object and lift up to pre pose gripper_fully_closed = self.closeGripper() self.adjustGripperCommand() if gripper_fully_closed: self.openGripper() self.moveTo(pre_pos, pre_rot, True) for i in range(10): pb.stepSimulation() else: self.moveTo(pos, rot, dynamic) self.holding_obj = self.getPickedObj(objects) self.moveToJ(self.home_positions_joint, dynamic) self.checkGripperClosed()
def calcWheelVelsAngle(self, pos, quat, lv, av): # Calculate the Z axis of the car body. rotMat = p.getMatrixFromQuaternion(quat) hMat = np.eye(4, dtype=np.float32) hMat[:3, :3] = np.array(rotMat).reshape((3, 3)) zVec = np.array([0, 0, 1, 1]) rotatedZ = np.matmul(hMat, zVec) # Calculate the angle of Z axis. theta = self.calcAngle(zVec, rotatedZ) yVec = np.array([0, 1, 0, 1]) rotatedY = np.matmul(hMat, yVec) cross = np.cross(zVec[:3], rotatedZ[:3]) yTheta = self.calcAngle(cross, rotatedY) if yTheta > math.pi / 2: theta = -theta print(theta) # PID dTheta = (theta - self.lastP) / self.ts self.lastP = theta self.inti = self.inti + theta * self.ts v = self.kp * theta + self.kd * dTheta + self.ki * self.inti res = self.mul * v print(res) return res, res
camInfo = p.getDebugVisualizerCamera() lastTime = time.time() lastControlTime = time.time() lastLidarTime = time.time() frame=0 while (True): nowTime = time.time() #render Camera at 10Hertz if (nowTime-lastTime>.1): ls = p.getLinkState(car,zed_camera_joint, computeForwardKinematics=True) camPos = ls[0] camOrn = ls[1] camMat = p.getMatrixFromQuaternion(camOrn) upVector = [0,0,1] forwardVec = [camMat[0],camMat[3],camMat[6]] #sideVec = [camMat[1],camMat[4],camMat[7]] camUpVec = [camMat[2],camMat[5],camMat[8]] camTarget = [camPos[0]+forwardVec[0]*10,camPos[1]+forwardVec[1]*10,camPos[2]+forwardVec[2]*10] camUpTarget = [camPos[0]+camUpVec[0],camPos[1]+camUpVec[1],camPos[2]+camUpVec[2]] viewMat = p.computeViewMatrix(camPos, camTarget, camUpVec) projMat = camInfo[3] #p.getCameraImage(320,200,viewMatrix=viewMat,projectionMatrix=projMat, flags=p.ER_NO_SEGMENTATION_MASK, renderer=p.ER_BULLET_HARDWARE_OPENGL) p.getCameraImage(320,200,viewMatrix=viewMat,projectionMatrix=projMat, renderer=p.ER_BULLET_HARDWARE_OPENGL) lastTime=nowTime nowControlTime = time.time() nowLidarTime = time.time()
def is_fallen(): global minitaur orientation = minitaur.getBaseOrientation() rotMat = p.getMatrixFromQuaternion(orientation) localUp = rotMat[6:] return np.dot(np.asarray([0, 0, 1]), np.asarray(localUp)) < 0
def talker(): global getMode, get_last_vel, myflags iter = 0 iter1 = 0 get_orientation = [] get_euler = [] get_matrix = [] get_velocity = [] get_invert = [] jointTorques = [] jointTorques1 = [] init_pos = [] middle_target = [] joint_Kp = [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10] joint_Kd = [0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2] stand_target = [ 0.0, -0.8, 1.6, 0.0, -0.8, 1.6, 0.0, -0.8, 1.6, 0.0, -0.8, 1.6 ] pub1 = rospy.Publisher('/get_js', JointState, queue_size=100) pub2 = rospy.Publisher('/imu_body', Imu, queue_size=100) pub3 = rospy.Publisher('/get_com', commandDes, queue_size=100) imu_msg = Imu() setJSMsg = JointState() com_msg = commandDes() freq = 400 rate = rospy.Rate(freq) # hz while not rospy.is_shutdown(): if myflags == 100: p.resetBasePositionAndOrientation(boxId, [0, 0, 0.2], [0, 0, 0, 1]) time.sleep(1) for j in range(16): force = 0 p.setJointMotorControl2(boxId, j, p.VELOCITY_CONTROL, force=force) get_orientation = [] pose_orn = p.getBasePositionAndOrientation(boxId) for i in range(4): get_orientation.append(pose_orn[1][i]) get_euler = p.getEulerFromQuaternion(get_orientation) get_velocity = p.getBaseVelocity(boxId) get_invert = p.invertTransform(pose_orn[0], pose_orn[1]) get_matrix = p.getMatrixFromQuaternion(get_invert[1]) # IMU data imu_msg.orientation.x = pose_orn[1][0] imu_msg.orientation.y = pose_orn[1][1] imu_msg.orientation.z = pose_orn[1][2] imu_msg.orientation.w = pose_orn[1][3] imu_msg.angular_velocity.x = get_matrix[0] * get_velocity[1][ 0] + get_matrix[1] * get_velocity[1][1] + get_matrix[ 2] * get_velocity[1][2] imu_msg.angular_velocity.y = get_matrix[3] * get_velocity[1][ 0] + get_matrix[4] * get_velocity[1][1] + get_matrix[ 5] * get_velocity[1][2] imu_msg.angular_velocity.z = get_matrix[6] * get_velocity[1][ 0] + get_matrix[7] * get_velocity[1][1] + get_matrix[ 8] * get_velocity[1][2] # calculate the acceleration of the robot linear_X = (get_velocity[0][0] - get_last_vel[0]) * freq linear_Y = (get_velocity[0][1] - get_last_vel[1]) * freq linear_Z = 9.8 + (get_velocity[0][2] - get_last_vel[2]) * freq imu_msg.linear_acceleration.x = get_matrix[0] * linear_X + get_matrix[ 1] * linear_Y + get_matrix[2] * linear_Z imu_msg.linear_acceleration.y = get_matrix[3] * linear_X + get_matrix[ 4] * linear_Y + get_matrix[5] * linear_Z imu_msg.linear_acceleration.z = get_matrix[6] * linear_X + get_matrix[ 7] * linear_Y + get_matrix[8] * linear_Z # joint data joint_state = p.getJointStates(boxId, motor_id_list) setJSMsg.position = [ joint_state[0][0], joint_state[1][0], joint_state[2][0], joint_state[3][0], joint_state[4][0], joint_state[5][0], joint_state[6][0], joint_state[7][0], joint_state[8][0], joint_state[9][0], joint_state[10][0], joint_state[11][0] ] setJSMsg.velocity = [ joint_state[0][1], joint_state[1][1], joint_state[2][1], joint_state[3][1], joint_state[4][1], joint_state[5][1], joint_state[6][1], joint_state[7][1], joint_state[8][1], joint_state[9][1], joint_state[10][1], joint_state[11][1] ] # com data com_msg.com_position = [pose_orn[0][0], pose_orn[0][1], pose_orn[0][2]] com_msg.com_velocity = [ get_velocity[0][0], get_velocity[0][1], get_velocity[0][2] ] get_last_vel.clear() get_last_vel = com_msg.com_velocity # stand up control if len(get_effort): print(get_effort) p.setJointMotorControlArray(bodyUniqueId=boxId, jointIndices=motor_id_list, controlMode=p.TORQUE_CONTROL, forces=get_effort) setJSMsg.header.stamp = rospy.Time.now() setJSMsg.name = [ "abduct_fr", "thigh_fr", "knee_fr", "abduct_fl", "thigh_fl", "knee_fl", "abduct_hr", "thigh_hr", "knee_hr", "abduct_hl", "thigh_hl", "knee_hl" ] if myflags % 2 == 0: pub2.publish(imu_msg) pub1.publish(setJSMsg) pub3.publish(com_msg) myflags = myflags + 1 p.setTimeStep(0.0015) p.stepSimulation() rate.sleep()
def pbvs(self, arm, goal_pos, goal_rot, kv=1.3, kw=0.8, eps_pos=0.005, eps_rot=0.05, vs_rate=20, plot_pose=False, print_err=False, perturb_jacobian=False, perturb_Jac_joint_mu=0.1, perturb_Jac_joint_sigma=0.1, perturb_orientation=False, mu_R=0.4, sigma_R=0.4, perturb_motion_R_mu=0.0, perturb_motion_R_sigma=0.0, perturb_motion_t_mu=0.0, perturb_motion_t_sigma=0.0, plot_result=False, pf_mode=False): # Initialize variables self.perturb_Jac_joint_mu = perturb_Jac_joint_mu self.perturb_Jac_joint_sigma = perturb_Jac_joint_sigma self.perturb_R_mu = mu_R self.perturb_R_sigma = sigma_R if arm == "right": tool = self.right_tool elif arm == "left": tool = self.left_tool else: print('''arm incorrect, input "left" or "right" ''') return # Initialize pos_plot_id for later use if pf_mode: cur_pos, cur_rot = SE32_trans_rot(self.cur_pose_est) else: cur_pos, cur_rot = SE32_trans_rot( self.Trc.inverse() * get_pose(self.robot, tool, SE3=True)) if plot_pose: pos_plot_id = self.draw_pose_cam(trans_rot2SE3(cur_pos, cur_rot)) # record end effector pose and time stamp at each iteration if plot_result: start = time.time() times = [] eef_pos = [] eef_rot = [] if pf_mode: motion = sp.SE3() ##### Control loop starts ##### while True: # If using particle filter and the hog computation has finished, exit pbvs control # and assign total estimated motion in eef frame if pf_mode and self.shared_pf_fin.value == 1: # eef Motion estimated from pose estimate of last iteration and current fk motion_truth = (self.Trc * self.cur_pose_est).inverse() * get_pose( self.robot, tool, SE3=True) # print("motion truth:", motion_truth) dR = sp.SO3.exp( np.random.normal(perturb_motion_R_mu, perturb_motion_R_sigma, 3)).matrix() dt = np.random.normal(perturb_motion_t_mu, perturb_motion_t_sigma, 3) # self.draw_pose_cam(motion) self.motion = motion_truth * sp.SE3(dR, dt) return # get current eef pose in camera frame if pf_mode: cur_pos, cur_rot = SE32_trans_rot(self.cur_pose_est) else: cur_pos, cur_rot = SE32_trans_rot( self.Trc.inverse() * get_pose(self.robot, tool, SE3=True)) if plot_pose: self.draw_pose_cam(trans_rot2SE3(cur_pos, cur_rot), uids=pos_plot_id) if plot_result: eef_pos.append(cur_pos) eef_rot.append(cur_rot) times.append(time.time() - start) cur_pos_inv, cur_rot_inv = p.invertTransform(cur_pos, cur_rot) # Pose of goal in camera frame pos_cg, rot_cg = p.multiplyTransforms(cur_pos_inv, cur_rot_inv, goal_pos, goal_rot) # Evaluate current translational and rotational error err_pos = np.linalg.norm(pos_cg) err_rot = np.linalg.norm(p.getAxisAngleFromQuaternion(rot_cg)[1]) if err_pos < eps_pos and err_rot < eps_rot: p.removeAllUserDebugItems() break else: if print_err: print("Error to goal, position: {:2f}, orientation: {:2f}". format(err_pos, err_rot)) Rsc = np.array(p.getMatrixFromQuaternion(cur_rot)).reshape(3, 3) # Perturb Rsc in SO(3) by a random variable in tangent space so(3) if perturb_orientation: dR = sp.SO3.exp( np.random.normal(self.perturb_R_mu, self.perturb_R_sigma, 3)) Rsc = Rsc.dot(dR.matrix()) twist_local = np.hstack( (np.array(pos_cg) * kv, np.array(quat2se3(rot_cg)) * kw)).reshape(6, 1) local2global = np.block([[Rsc, np.zeros((3, 3))], [np.zeros((3, 3)), Rsc]]) twist_global = local2global.dot(twist_local) start_loop = time.time() self.cartesian_vel_control(arm, np.asarray(twist_global), 1 / vs_rate, perturb_jacobian=perturb_jacobian) if pf_mode: delta_t = time.time() - start_loop motion = motion * sp.SE3.exp(twist_local * delta_t) # self.draw_pose_cam(motion) self.goal_reached = True print("PBVS goal achieved!") if plot_result: eef_pos = np.array(eef_pos) eef_rot_rpy = np.array( [p.getEulerFromQuaternion(quat) for quat in eef_rot]) fig, axs = plt.subplots(3, 2) sub_titles = [['x', 'roll'], ['y', 'pitch'], ['z', 'yaw']] fig.suptitle( "Position Based Visual Servo End Effector Pose - time plot") for i in range(3): axs[i, 0].plot(times, eef_pos[:, i] * 100) axs[i, 0].plot(times, goal_pos[i] * np.ones(len(times)) * 100) axs[i, 0].legend([sub_titles[i][0], 'goal']) axs[i, 0].set_xlabel('Time(s)') axs[i, 0].set_ylabel('cm') goal_rpy = p.getEulerFromQuaternion(goal_rot) for i in range(3): axs[i, 1].plot(times, eef_rot_rpy[:, i] * 180 / np.pi) axs[i, 1].plot(times, goal_rpy[i] * np.ones(len(times)) * 180 / np.pi) axs[i, 1].legend([sub_titles[i][1], 'goal']) axs[i, 1].set_xlabel('Time(s)') axs[i, 1].set_ylabel('deg') for ax in axs.flat: ax.set(xlabel='time') plt.show()
def matrix_from_quat(quat): return np.array(p.getMatrixFromQuaternion(quat, physicsClientId=CLIENT)).reshape(3, 3)
def trans_rot2SE3(trans, rot): rotm = np.array(p.getMatrixFromQuaternion(rot)).reshape(3, 3) return sp.SE3(rotm, np.array(trans))
seg = np.reshape(images[4], (height, width)) block_mask = object_mask_from_seg(object_uid, seg) posmap = get_posmap(near, far, view_matrix, projection_matrix, height, width, depth_buffer) block_posmap = posmap[block_mask != 0] v = longitudinal_direction(block_posmap) # blockの長手方向を描画 p.addUserDebugLine(b_pos + v / 10, b_pos - v / 10, [1, 0, 0]) # world -> eefの同時変換行列を計算 eef_base_pos, eef_base_orn = p.getLinkState(kukaId, kukaEndEffectorIndex)[4:6] world_to_eef_base = np.eye(4) world_to_eef_base[0:3, 0:3] = np.array( p.getMatrixFromQuaternion(eef_base_orn)).reshape(3, 3) world_to_eef_base[:3, 3] = eef_base_pos # yawの回転角の計算, eefのy軸方向にブロックの長手方向が一致すればいい v_in_eef = np.linalg.inv(world_to_eef_base).dot(np.hstack( [v, 1]))[:3] - np.linalg.inv(world_to_eef_base).dot( np.array([0, 0, 0, 1]))[:3] v_in_eef = v_in_eef / np.linalg.norm(v_in_eef) e_y = np.array([0, 1., 0]) yaw_rot_diff = np.arccos(np.dot(v_in_eef, e_y)) yaw_now = p.getEulerFromQuaternion(eef_base_orn)[2] eef_orn = [0, -np.pi, yaw_now] # eefが下向き if v_in_eef[0] < 0: eef_orn[2] -= yaw_rot_diff else: eef_orn[2] += yaw_rot_diff # TODO : eef_orn[2]をjoint_limitに収める処理を追加?
parentLinkIndex=-1, replaceItemUniqueId=lines[i]) lines[i] = textUid #keep the gripper centered/symmetric b = p.getJointState(pr2_gripper, 2)[0] p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3) events = p.getVREvents() for e in (events): if e[CONTROLLER_ID] == uiControllerId: p.resetBasePositionAndOrientation(uiCube, e[POSITION], e[ORIENTATION]) pos = e[POSITION] orn = e[ORIENTATION] lineFrom = pos mat = p.getMatrixFromQuaternion(orn) dir = [mat[0], mat[3], mat[6]] to = [pos[0] + dir[0] * rayLen, pos[1] + dir[1] * rayLen, pos[2] + dir[2] * rayLen] hit = p.rayTest(lineFrom, to) oldRay = pointRay color = [1, 1, 0] width = 3 #pointRay = p.addUserDebugLine(lineFrom,to,color,width,lifeTime=1) #if (oldRay>=0): # p.removeUserDebugItem(oldRay) if (hit): #if (hit[0][0]>=0): hitObjectUid = hit[0][0] linkIndex = hit[0][1] if (hitObjectUid >= 0):
def apply_command(self, cmd, velocity=True, local_coord=True): """ Applies the command to the sensor. Args: cmd (list) : Position and orientation commands. velocity (bool) : Set to true if the commands are velocity, they are assumed to be position otherwise. local_coord (bool) : Set to false to use global coordinates. """ if self._virtual_links: for j in range(p.getNumJoints(self._sensor_id)): if velocity: p.setJointMotorControl2(self._sensor_id, j, p.VELOCITY_CONTROL, targetPosition=0, targetVelocity=cmd[j], velocityGain=1., force=self._max_force) else: p.setJointMotorControl2(self._sensor_id, j, p.POSITION_CONTROL, targetPosition=cmd[j], targetVelocity=0, positionGain=1, velocityGain=1, force=self._max_force) else: if velocity: delta_position = np.array(cmd[0:3]) * config.TIME_STEP delta_orientation = np.array(cmd[3:6]) * config.TIME_STEP base_position, base_orientation = p.getBasePositionAndOrientation( self._sensor_id) if local_coord: rot_mat = p.getMatrixFromQuaternion(base_orientation) rot_mat = np.array(rot_mat).reshape(3, 3) new_position = rot_mat.dot(delta_position) + np.array( base_position) else: new_position = delta_position + np.array(base_position) base_orientation = np.array( p.getEulerFromQuaternion(base_orientation)) new_orientation = (base_orientation + delta_orientation).tolist() new_orientation = p.getQuaternionFromEuler(new_orientation) else: assert not local_coord, "Position controller only works with global coordinates." new_position = cmd[0:3] new_orientation = p.getQuaternionFromEuler(cmd[3:6]) if self._constrained: p.changeConstraint(self._sensor_constraint, new_position, new_orientation, maxForce=self._max_force) else: p.resetBasePositionAndOrientation(self._sensor_id, new_position, new_orientation)
nowhere = False while (path.rad < radius + .5): # maxForce = p.readUserDebugParameter(maxForceSlider) # targetVelocity = p.readUserDebugParameter(targetVelocitySlider) # steeringAngle = p.readUserDebugParameter(steeringSlider) maxForce = 10. targetVelocity = -5 steeringAngle = 0 #########---------------START Obstacle Avoidance----------######## position, orientation = p.getBasePositionAndOrientation(car) matrix = p.getMatrixFromQuaternion(orientation) matrix = np.reshape(matrix, (3, 3)) src = np.array(position) src = src + np.matmul(matrix,offset) path.calcDist(src) h = 10 rays_src = np.repeat([src], num_rays, axis=0) orn = np.matmul(matrix, rot) #rotates unit vector y to -x rays_end = np.matmul(orn, rays) # unit vector in direction of minitaur rays_end = (rays_end + src[:, None]).T rays_info = p.rayTestBatch(rays_src.tolist(), rays_end.tolist())
colors[0] = [0, 0, 0] colors[1] = [0.5, 0, 0] colors[2] = [0, 0.5, 0] colors[3] = [0, 0, 0.5] colors[4] = [0.5, 0.5, 0.] colors[5] = [.5, .5, .5] p.startStateLogging(p.STATE_LOGGING_VR_CONTROLLERS, "vr_hmd.bin", deviceTypeFilter=p.VR_DEVICE_HMD) p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, "generic_data.bin") p.startStateLogging(p.STATE_LOGGING_CONTACT_POINTS, "contact_points.bin") while True: events = p.getVREvents(p.VR_DEVICE_HMD + p.VR_DEVICE_GENERIC_TRACKER) for e in (events): pos = e[POSITION] mat = p.getMatrixFromQuaternion(e[ORIENTATION]) dir0 = [mat[0], mat[3], mat[6]] dir1 = [mat[1], mat[4], mat[7]] dir2 = [mat[2], mat[5], mat[8]] lineLen = 0.1 dir = [-mat[2], -mat[5], -mat[8]] to = [pos[0] + lineLen * dir[0], pos[1] + lineLen * dir[1], pos[2] + lineLen * dir[2]] toX = [pos[0] + lineLen * dir0[0], pos[1] + lineLen * dir0[1], pos[2] + lineLen * dir0[2]] toY = [pos[0] + lineLen * dir1[0], pos[1] + lineLen * dir1[1], pos[2] + lineLen * dir1[2]] toZ = [pos[0] + lineLen * dir2[0], pos[1] + lineLen * dir2[1], pos[2] + lineLen * dir2[2]] p.addUserDebugLine(pos, toX, [1, 0, 0], 1) p.addUserDebugLine(pos, toY, [0, 1, 0], 1) p.addUserDebugLine(pos, toZ, [0, 0, 1], 1) p.addUserDebugLine(pos, to, [0.5, 0.5, 0.], 1, 3)
b = p.getJointState(pr2_gripper, 2)[0] p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3) events = p.getVREvents() for e in (events): if e[CONTROLLER_ID] == uiControllerId: p.resetBasePositionAndOrientation(uiCube, e[POSITION], e[ORIENTATION]) pos = e[POSITION] orn = e[ORIENTATION] lineFrom = pos mat = p.getMatrixFromQuaternion(orn) dir = [mat[0], mat[3], mat[6]] to = [ pos[0] + dir[0] * rayLen, pos[1] + dir[1] * rayLen, pos[2] + dir[2] * rayLen ] hit = p.rayTest(lineFrom, to) oldRay = pointRay color = [1, 1, 0] width = 3 #pointRay = p.addUserDebugLine(lineFrom,to,color,width,lifeTime=1) #if (oldRay>=0): # p.removeUserDebugItem(oldRay) if (hit): #if (hit[0][0]>=0):
def _dslPIDPositionControl(self, control_timestep, cur_pos, cur_quat, cur_vel, target_pos, target_rpy, target_vel): """DSL's CF2.x PID position control. Parameters ---------- control_timestep : float The time step at which control is computed. cur_pos : ndarray (3,1)-shaped array of floats containing the current position. cur_quat : ndarray (4,1)-shaped array of floats containing the current orientation as a quaternion. cur_vel : ndarray (3,1)-shaped array of floats containing the current velocity. target_pos : ndarray (3,1)-shaped array of floats containing the desired position. target_rpy : ndarray (3,1)-shaped array of floats containing the desired orientation as roll, pitch, yaw. target_vel : ndarray (3,1)-shaped array of floats containing the desired velocity. Returns ------- float The target thrust along the drone z-axis. ndarray (3,1)-shaped array of floats containing the target roll, pitch, and yaw. float The current position error. """ cur_rotation = np.array(p.getMatrixFromQuaternion(cur_quat)).reshape( 3, 3) pos_e = target_pos - cur_pos vel_e = target_vel - cur_vel self.integral_pos_e = self.integral_pos_e + pos_e * control_timestep self.integral_pos_e = np.clip(self.integral_pos_e, -2., 2.) self.integral_pos_e[2] = np.clip(self.integral_pos_e[2], -0.15, .15) #### PID target thrust ##################################### target_thrust = np.multiply(self.P_COEFF_FOR, pos_e) \ + np.multiply(self.I_COEFF_FOR, self.integral_pos_e) \ + np.multiply(self.D_COEFF_FOR, vel_e) + np.array([0, 0, self.GRAVITY]) scalar_thrust = max(0., np.dot(target_thrust, cur_rotation[:, 2])) thrust = (math.sqrt(scalar_thrust / (4 * self.KF)) - self.PWM2RPM_CONST) / self.PWM2RPM_SCALE target_z_ax = target_thrust / np.linalg.norm(target_thrust) target_x_c = np.array( [math.cos(target_rpy[2]), math.sin(target_rpy[2]), 0]) target_y_ax = np.cross(target_z_ax, target_x_c) / np.linalg.norm( np.cross(target_z_ax, target_x_c)) target_x_ax = np.cross(target_y_ax, target_z_ax) target_rotation = (np.vstack([target_x_ax, target_y_ax, target_z_ax])).transpose() #### Target rotation ####################################### target_euler = (Rotation.from_matrix(target_rotation)).as_euler( 'XYZ', degrees=False) if np.any(np.abs(target_euler) > math.pi): print( "\n[ERROR] ctrl it", self.control_counter, "in Control._dslPIDPositionControl(), values outside range [-pi,pi]" ) return thrust, target_euler, pos_e