Python pybullet.getEulerFromQuaternion方法代码示例

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getEulerFromQuaternion [as 别名]
def get_observation(self):
        observation = []
        observation_lim = []

        # get object position
        obj_pos, obj_orn = p.getBasePositionAndOrientation(self.obj_id, physicsClientId=self._physics_client_id)
        observation.extend(list(obj_pos))
        observation_lim.extend(self._ws_lim)

        if self._control_eu_or_quat is 0:
            obj_euler = p.getEulerFromQuaternion(obj_orn)  # roll, pitch, yaw
            observation.extend(list(obj_euler))
            observation_lim.extend([[-m.pi, m.pi], [-m.pi, m.pi], [-m.pi, m.pi]])
        else:
            observation.extend(list(obj_orn))
            observation_lim.extend([[-1, 1], [-1, 1], [-1, 1], [-1, 1]])

        return observation, observation_lim 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getEulerFromQuaternion [as 别名]
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 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getEulerFromQuaternion [as 别名]
def getObservation(self):
    observation = []
    state = p.getLinkState(self.kukaUid,self.kukaGripperIndex)
    pos = state[0]
    orn = state[1]
    euler = p.getEulerFromQuaternion(orn)
        
    observation.extend(list(pos))
    observation.extend(list(euler))
    
    return observation 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getEulerFromQuaternion [as 别名]
def rpy(self):
		return pybullet.getEulerFromQuaternion(self.body_part.current_orientation()) 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getEulerFromQuaternion [as 别名]
def updateImuMeasurments(self):
        """Get IMU measurements from simulation and convert to usable format"""
        imu_info = p.getLinkState(self.body, self.imu, computeLinkVelocity=1)
        self.imuMeasurements.position = imu_info[0]
        self.imuMeasurements.orientation = p.getEulerFromQuaternion(imu_info[1])
        self.imuMeasurements.velocity = imu_info[6] 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getEulerFromQuaternion [as 别名]
def getRobotPose(self):
        """Gets the robot (origin) position

        Returns:
            (tuple(3), tuple(3)) -- (x,y,z), (roll, pitch, yaw)
        """
        pose = p.getBasePositionAndOrientation()
        return (pose[0], p.getEulerFromQuaternion(pose[1])) 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getEulerFromQuaternion [as 别名]
def getFrames(self):
        """Gets the available frames in the current robot model
        
        Returns:
            dict -- dict of str -> (pos, orientation)
        """
        frames = {}

        for name in self.frames.keys():
            jointState = p.getLinkState(self.robot, self.frames[name])
            pos = jointState[0]
            orientation = p.getEulerFromQuaternion(jointState[1])
            frames[name] = [pos, orientation]

        return frames 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getEulerFromQuaternion [as 别名]
def euler_from_quat(quat):
        quat = list(quat)
        euler = p.getEulerFromQuaternion(quat)
        return np.array(euler, dtype=np.float32) 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getEulerFromQuaternion [as 别名]
def get_body_euler(body):
        _, quat = p.getBasePositionAndOrientation(body)
        euler = p.getEulerFromQuaternion(quat)
        return np.array(euler, dtype=np.float32) 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getEulerFromQuaternion [as 别名]
def get_cstr_dof(cstr):
        _, _, _, _, _, _, _, pos, _, quat, max_force = p.getConstraintInfo(cstr)
        pos = np.array(pos, dtype=np.float32)
        euler = p.getEulerFromQuaternion(quat)
        euler = np.array(euler, dtype=np.float32)
        return pos, euler 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getEulerFromQuaternion [as 别名]
def step(self, action):
        self.take_step(action, robot_arm='right', gains=self.config('robot_gains'), forces=self.config('robot_forces'), human_gains=0.0005)

        robot_force_on_human, cup_force_on_human = self.get_total_force()
        total_force_on_human = robot_force_on_human + cup_force_on_human
        reward_water, water_mouth_velocities, water_hit_human_reward = self.get_water_rewards()
        end_effector_velocity = np.linalg.norm(p.getBaseVelocity(self.cup, physicsClientId=self.id)[0])
        obs = self._get_obs([cup_force_on_human], [robot_force_on_human, cup_force_on_human])

        # Get human preferences
        preferences_score = self.human_preferences(end_effector_velocity=end_effector_velocity, total_force_on_human=robot_force_on_human, tool_force_at_target=cup_force_on_human, food_hit_human_reward=water_hit_human_reward, food_mouth_velocities=water_mouth_velocities)

        cup_pos, cup_orient = p.getBasePositionAndOrientation(self.cup, physicsClientId=self.id)
        cup_pos, cup_orient = p.multiplyTransforms(cup_pos, cup_orient, [0, 0.06, 0], p.getQuaternionFromEuler([np.pi/2.0, 0, 0], physicsClientId=self.id), physicsClientId=self.id)
        cup_top_center_pos, _ = p.multiplyTransforms(cup_pos, cup_orient, self.cup_top_center_offset, [0, 0, 0, 1], physicsClientId=self.id)
        reward_distance = -np.linalg.norm(self.target_pos - np.array(cup_top_center_pos)) # Penalize distances between top of cup and mouth
        reward_action = -np.sum(np.square(action)) # Penalize actions
        # Encourage robot to have a tilted end effector / cup
        cup_euler = p.getEulerFromQuaternion(cup_orient, physicsClientId=self.id)
        reward_tilt = -abs(cup_euler[0] + np.pi/2) if self.robot_type == 'jaco' else -abs(cup_euler[0] - np.pi/2)

        reward = self.config('distance_weight')*reward_distance + self.config('action_weight')*reward_action + self.config('cup_tilt_weight')*reward_tilt + self.config('drinking_reward_weight')*reward_water + preferences_score

        if self.gui and reward_water != 0:
            print('Task success:', self.task_success, 'Water reward:', reward_water)

        info = {'total_force_on_human': total_force_on_human, 'task_success': int(self.task_success >= self.total_water_count*self.config('task_success_threshold')), 'action_robot_len': self.action_robot_len, 'action_human_len': self.action_human_len, 'obs_robot_len': self.obs_robot_len, 'obs_human_len': self.obs_human_len}
        done = False

        return obs, reward, done, info 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getEulerFromQuaternion [as 别名]
def getObservation(self):
        """
        Returns the position and angle of the effector
        :return: ([float])
        """
        observation = []
        state = p.getLinkState(self.kuka_uid, self.kuka_gripper_index)
        pos = state[0]
        orn = state[1]
        euler = p.getEulerFromQuaternion(orn)

        observation.extend(list(pos))
        observation.extend(list(euler))

        return observation 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getEulerFromQuaternion [as 别名]
def get_orientation_eulerian(self):
        return p.getEulerFromQuaternion(self.get_orientation()) 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getEulerFromQuaternion [as 别名]
def apply_action(self, a):
        yaw= a
        pos, ornQuaternion = p.getBasePositionAndOrientation(self.robot_ids[0])
        ornEuler = p.getEulerFromQuaternion(ornQuaternion)
        currAngle = ornEuler[2]-self.config['yaw_constant']*yaw
        print(pos)
        print(ornEuler)
        pos = [pos[0]+ self.config['vx_constant']*self.vx*math.cos(currAngle)+random.uniform(self.config['wind_limits'][0], self.config['wind_limits'][1]), 
        pos[1]+self.config['vx_constant']*self.vx*math.sin(currAngle)+random.uniform(self.config['wind_limits'][0], self.config['wind_limits'][1]), self.height]
        pitch = min(max( ornEuler[0] + random.uniform(self.config['d_roll_per_step'][0], self.config['d_roll_per_step'][1]), self.config['roll_limits'][0]), self.config['roll_limits'][1])
        ornEuler = [pitch, ornEuler[1], currAngle]
        ornQuaternion = p.getQuaternionFromEuler(ornEuler)
        self.robot_body.set_pose(pos, ornQuaternion)
        return pos[0], pos[1], ornEuler[2], self.height, self.vx 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getEulerFromQuaternion [as 别名]
def rpy(self):
        return p.getEulerFromQuaternion(self.body_part.get_orientation())