Facebook
From jczhang, 2 Years ago, written in Python.
Embed
Download Paste or View Raw
Hits: 49
  1. #!/usr/bin/env python3
  2. # -*- encoding: utf-8 -*-
  3. '''
  4. @File    :   taskFirst.py
  5. @Time    :   2021/06/10 5:00:00
  6. @Author  :   JC Zhang
  7. @Version :   1.0
  8. @Contact :   [email protected]
  9. @License :   GUN
  10. @Desc    :   None
  11. '''
  12.  
  13. # here put the import lib
  14.  
  15. import linecache
  16. import pybullet as p
  17. import pybullet_data
  18. import os
  19. import sys
  20. import gym
  21. from gym import spaces
  22. from gym.utils import seeding
  23. import numpy as np
  24. from math import sqrt
  25. import random
  26. import time
  27. from numpy import arange
  28. import logging
  29. import math
  30. from termcolor import colored
  31.  
  32. #### 一些变量 ######k
  33. LOGGING_LEVEL = logging.INFO
  34. # is_render=False
  35. # is_good_view=False   #这个的作用是在step时加上time.sleep(),把机械比的动作放慢,看的更清,但是会降低训练速度
  36. #########################
  37.  
  38. # logging.basicConfig(
  39. #     level=LOGGING_LEVEL,
  40. #     format='%(asctime)s - %(threadName)s - %(pathname)s[line:%(lineno)d] - %(levelname)s: %(message)s',
  41. #     filename='../logs/reach_env.log'.format(time.strftime("%Y_%m_%d_%H_%M_%S", time.localtime())),
  42. #     filemode='w')
  43. # logger = logging.getLogger(__name__)
  44. # env_logger=logging.getLogger('env.py')
  45.  
  46. # logging模块的使用
  47. # 级别                何时使用
  48. # DEBUG       细节信息,仅当诊断问题时适用。
  49. # INFO        确认程序按预期运行
  50. # WARNING     表明有已经或即将发生的意外(例如:磁盘空间不足)。程序仍按预期进行
  51. # ERROR       由于严重的问题,程序的某些功能已经不能正常执行
  52. # CRITICAL    严重的错误,表明程序已不能继续执行
  53.  
  54.  
  55. class KukaReachEnv(gym.Env):
  56.     metadata = {
  57.         'render.modes': ['human', 'rgb_array'],
  58.         'video.frames_per_second': 50
  59.     }
  60.     max_steps_one_episode = 1000
  61.  
  62.     def __init__(self, is_render=False, is_good_view=False):
  63.  
  64.         self.is_render = is_render
  65.         self.is_good_view = is_good_view
  66.  
  67.         if self.is_render:
  68.             p.connect(p.GUI)
  69.         else:
  70.             p.connect(p.DIRECT)
  71.  
  72.         self.x_low_obs = 0.0
  73.         self.x_high_obs = 0.5
  74.         self.y_low_obs = -0.3
  75.         self.y_high_obs = 0.3
  76.         self.z_low_obs = 0
  77.         self.z_high_obs = 0.55
  78.  
  79.         self.x_mid = (self.x_low_obs + self.x_high_obs) / 2
  80.         self.y_mid = 0
  81.  
  82.         self.x_low_action = 0.0
  83.         self.x_high_action = 0.5
  84.         self.y_low_action = -0.6
  85.         self.y_high_action = 0.6
  86.         self.z_low_action = -0.6
  87.         self.z_high_action = 0.3
  88.  
  89.         p.resetDebugVisualizerCamera(cameraDistance=1.5,
  90.                                      cameraYaw=0,
  91.                                      cameraPitch=-40,
  92.                                      cameraTargetPosition=[0.55, -0.35, 0.2])
  93.  
  94.         self.action_space = spaces.Box(low=np.array(
  95.             [self.x_low_action, self.y_low_action, self.z_low_action]),
  96.             high=np.array([
  97.                 self.x_high_action,
  98.                 self.y_high_action,
  99.                 self.z_high_action]),
  100.             dtype=np.float32)
  101.         self.observation_space = spaces.Box(
  102.             low=np.array([self.x_low_obs, self.y_low_obs, self.z_low_obs]),
  103.             high=np.array([self.x_high_obs, self.y_high_obs, self.z_high_obs]),
  104.             dtype=np.float32)
  105.         self.step_counter = 0
  106.  
  107.         self.urdf_root_path = pybullet_data.getDataPath()
  108.         # lower limits for null space
  109.         self.lower_limits = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
  110.         # upper limits for null space
  111.         self.upper_limits = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05]
  112.         # joint ranges for null space
  113.         self.joint_ranges = [5.8, 4, 5.8, 4, 5.8, 4, 6]
  114.         # restposes for null space
  115.         self.rest_poses = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0]
  116.         # joint damping coefficents
  117.         self.joint_damping = [
  118.             0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001
  119.         ]
  120.  
  121.         self.init_joint_positions = [
  122.             0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684,
  123.             -0.006539
  124.         ]
  125.  
  126.         self.orientation = p.getQuaternionFromEuler(
  127.             [0., -math.pi, math.pi / 2.])
  128.  
  129.  
  130.         self.seed()
  131.         self.reset()
  132.  
  133.     def seed(self, seed=None):
  134.         self.np_random, seed = seeding.np_random(seed)
  135.         return [seed]
  136.  
  137.     def reset(self):
  138.         # p.connect(p.GUI)
  139.         self.step_counter = 0
  140.  
  141.         p.resetSimulation()
  142.         # p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
  143.         self.terminated = False
  144.         p.setGravity(0, 0, -10)
  145.  
  146.         # 外界大的边界 蓝色
  147.         # 竖线
  148.         p.addUserDebugLine(
  149.             lineFromXYZ=[self.x_low_action, self.y_low_action, 0],
  150.             lineToXYZ=[self.x_low_action, self.y_high_action, 0],
  151.             lineColorRGB=[1, 0, 0]
  152.         )
  153.  
  154.         p.addUserDebugLine(
  155.             lineFromXYZ=[self.x_high_action, self.y_low_action, 0],
  156.             lineToXYZ=[self.x_high_action, self.y_high_action, 0],
  157.             lineColorRGB=[0, 0, 255]
  158.         )
  159.         # 横线
  160.  
  161.         p.addUserDebugLine(
  162.             lineFromXYZ=[self.x_low_action, self.y_low_action, 0],
  163.             lineToXYZ=[self.x_high_action, self.y_low_action, 0],
  164.             lineColorRGB=[0, 0, 255]
  165.         )
  166.  
  167.         p.addUserDebugLine(
  168.             lineFromXYZ=[self.x_low_action, self.y_high_action, 0],
  169.             lineToXYZ=[self.x_high_action, self.y_high_action, 0],
  170.             lineColorRGB=[0, 0, 255]
  171.         )
  172.  
  173.         # 内部小边界
  174.  
  175.         #两条较长的横线
  176.         p.addUserDebugLine(
  177.             lineFromXYZ=[self.x_low_obs - 0.2, self.y_low_obs, 0],
  178.             lineToXYZ=[self.x_high_obs + 0.2, self.y_low_obs, 0],
  179.             lineColorRGB=[255, 97, 0]
  180.         )
  181.  
  182.         p.addUserDebugLine(
  183.             lineFromXYZ=[self.x_low_obs - 0.2, self.y_high_obs, 0],
  184.             lineToXYZ=[self.x_high_obs + 0.2, self.y_high_obs, 0],
  185.             lineColorRGB=[255, 97, 0]
  186.         )
  187.         # 一条较长的竖线
  188.         p.addUserDebugLine(
  189.             lineFromXYZ=[(self.x_low_obs + self.x_high_obs) / 2, self.y_low_action - 0.2, 0],
  190.             lineToXYZ=[(self.x_low_obs + self.x_high_obs) / 2, self.y_high_action + 0.2, 0],
  191.             lineColorRGB=[255, 96, 0]
  192.         )
  193.  
  194.         p.loadURDF(os.path.join(self.urdf_root_path, "plane.urdf"), basePosition=[0, 0, 0])
  195.         self.kuka_id = p.loadURDF(os.path.join(self.urdf_root_path,"kuka_iiwa/model.urdf"), useFixedBase=True)
  196.         #p.loadURDF(os.path.join(self.urdf_root_path, "table/table.urdf"), basePosition=[0.3, 0, 0], globalScaling=2)
  197.         # p.loadURDF(os.path.join(self.urdf_root_path, "tray/traybox.urdf"),basePosition=[0.55,0,0])
  198.         # object_id=p.loadURDF(os.path.join(self.urdf_root_path, "random_urdfs/000/000.urdf"), basePosition=[0.53,0,0.02])
  199.         self.object_id = p.loadURDF(os.path.join(self.urdf_root_path, "random_urdfs/000/000.urdf"),
  200.                                     basePosition=[random.uniform(self.x_low_obs, self.x_mid),
  201.                                                   random.uniform(self.y_low_obs, self.y_mid),
  202.                                                   0.01])
  203.  
  204.         self.num_joints = p.getNumJoints(self.kuka_id)
  205.  
  206.  
  207.         for i in range(self.num_joints):
  208.             p.resetJointState(
  209.                 bodyUniqueId=self.kuka_id,
  210.                 jointIndex=i,
  211.                 targetValue=self.init_joint_positions[i],
  212.             )
  213.         print(p.getLinkState(self.kuka_id, self.num_joints - 1)[4])
  214.         self.robot_pos_obs = p.getLinkState(self.kuka_id,
  215.                                             self.num_joints - 1)[4]
  216.         # logging.debug("init_pos={}\n".format(p.getLinkState(self.kuka_id,self.num_joints-1)))
  217.         p.stepSimulation()
  218.         self.object_pos = p.getBasePositionAndOrientation(self.object_id)[0]
  219.         return np.array(self.object_pos).astype(np.float32)
  220.         # return np.array(self.robot_pos_obs).astype(np.float32)
  221.  
  222.     def getInversePoisition(self, Uid, position_desired, orientation_desired=[]):
  223.         joints_info = []
  224.         joint_damping = []
  225.         joint_ll = []
  226.         joint_ul = []
  227.         useOrientaion = len(orientation_desired)
  228.         # for i in range(24):
  229.         #    joints_info.append(p.getJointInfo(Uid, i))
  230.         kukaEndEffectorIndex = 6
  231.         #numJoints = p.getNumJoints(Uid)
  232.         useNullSpace = 1
  233.         ikSolver = 1
  234.         pos = [position_desired[0], position_desired[1], position_desired[2]]
  235.         if useOrientaion:
  236.             orn = p.getQuaternionFromEuler([orientation_desired[0], orientation_desired[1], orientation_desired[2]])
  237.         if (useNullSpace == 1):
  238.             if (useOrientaion == 1):
  239.                 jointPoses = p.calculateInverseKinematics(Uid, kukaEndEffectorIndex, pos, orn)
  240.             else:
  241.                 jointPoses = p.calculateInverseKinematics(Uid, kukaEndEffectorIndex, pos, lowerLimits=joint_ll, upperLimits=joint_ul)
  242.         else:
  243.             if (useOrientaion == 1):
  244.                 jointPoses = p.calculateInverseKinematics(Uid, kukaEndEffectorIndex, pos, orn, solver=ikSolver, maxNumIterations=10, residualThreshold=.01)
  245.             else:
  246.                 jointPoses = p.calculateInverseKinematics(Uid, kukaEndEffectorIndex, pos, solver=ikSolver)
  247.             p.getJointInfo()
  248.         return jointPoses
  249.  
  250.     def get_to_place(self, pos):
  251.         orn = []
  252.         jointPoses = self.getInversePoisition(self.kuka_id, pos)
  253.         for i in range(7):
  254.             p.setJointMotorControl2(bodyIndex=self.kuka_id,
  255.                                     jointIndex=i,
  256.                                     controlMode=p.POSITION_CONTROL,
  257.                                     targetPosition=jointPoses[i - 3],
  258.                                     targetVelocity=0,
  259.                                     force=500,
  260.                                     positionGain=0.03,
  261.                                     velocityGain=1)
  262.         return
  263.  
  264.     def step(self, action):
  265.         # TODO  fix 'for'
  266.  
  267.         for i in range(1000):
  268.             p.stepSimulation()
  269.             self.get_to_place(action)
  270.             time.sleep(1 / 20)
  271.  
  272.  
  273.         if self.is_good_view:
  274.             time.sleep(0.05)
  275.  
  276.         return self._reward()
  277.  
  278.         '''
  279.        stepNum = 5
  280.        robot7StartPos = p.getLinkState(self.kuka_id, self.num_joints - 1)[4]
  281.        robot7EndPos = np.array([0.1, 0.1, 0.1])
  282.        robot7EndOrientation = p.getQuaternionFromEuler([0, 0, 0])
  283.        startPos = np.array(robot7StartPos)
  284.        endPos = robot7EndPos
  285.        stepArray = (endPos - startPos) / stepNum
  286.        p.resetBasePositionAndOrientation(self.kuka_id, [0, 0, 0], robot7EndOrientation)
  287.        for i in range(stepNum):
  288.            robotStepPos = list(stepArray + startPos)
  289.            target = p.calculateInverseKinematics(self.kuka_id, 6, robotStepPos, targetOrientation=robot7EndPos)
  290.            p.setJointMotorControlArray(self.kuka_id, range(7), p.POSITION_CONTROL, targetPositions=target)
  291.            for j in range(100):
  292.                p.stepSimulation()
  293.                time.sleep(1. / 100.)
  294.            startPos = np.array(robotStepPos)
  295.  
  296.        if self.is_good_view:
  297.            time.sleep(0.05)
  298.  
  299.        return self._reward()
  300.        '''
  301.  
  302.     def _reward(self):
  303.  
  304.         # 一定注意是取第4个值,请参考pybullet手册的这个函数返回值的说明
  305.         self.robot_state = p.getLinkState(self.kuka_id, self.num_joints - 1)[4]
  306.         # self.object_state=p.getBasePositionAndOrientation(self.object_id)
  307.         # self.object_state=np.array(self.object_state).astype(np.float32)
  308.         #
  309.         self.object_state = np.array(
  310.             p.getBasePositionAndOrientation(self.object_id)[0]).astype(np.float32)
  311.  
  312.         square_dx = (self.robot_state[0] - self.object_state[0])**2
  313.         square_dy = (self.robot_state[1] - self.object_state[1])**2
  314.         square_dz = (self.robot_state[2] - self.object_state[2])**2
  315.  
  316.         # 用机械臂末端和物体的距离作为奖励函数的依据
  317.         self.distance = sqrt(square_dx + square_dy + square_dz)
  318.         # print(self.distance)
  319.  
  320.         x = self.robot_state[0]
  321.         y = self.robot_state[1]
  322.         z = self.robot_state[2]
  323.  
  324.         # 如果机械比末端超过了obs的空间,也视为done,而且会给予一定的惩罚
  325.         '''
  326.        terminated = bool(x < self.x_low_obs or x > self.x_high_obs
  327.                          or y < self.y_low_obs or y > self.y_high_obs
  328.                          or z < self.z_low_obs or z > self.z_high_obs)
  329.        '''
  330.  
  331.         terminated = 1
  332.         if terminated:
  333.             reward = -0.1
  334.             self.terminated = True
  335.  
  336.         # 如果机械臂一直无所事事,在最大步数还不能接触到物体,也需要给一定的惩罚
  337.         elif self.step_counter > self.max_steps_one_episode:
  338.             reward = -0.1
  339.             self.terminated = True
  340.  
  341.         elif self.distance < 0.1:
  342.             reward = 1
  343.             self.terminated = True
  344.         else:
  345.             reward = 0
  346.             self.terminated = False
  347.  
  348.         info = {'distance:', self.distance}
  349.         # self.observation=self.robot_state
  350.         self.observation = self.object_state
  351.         return np.array(self.observation).astype(
  352.             np.float32), reward, self.terminated, info, np.array(self.robot_state).astype(
  353.             np.float32)
  354.  
  355.     def close(self):
  356.         p.disconnect()
  357.  
  358.  
  359. def getContent(cnt):
  360.     a, b, c = linecache.getline('pos.txt', cnt).split(",")
  361.     return float(a), float(b), float(c)
  362.  
  363.  
  364. def getCount():
  365.     cnt = len(open(r'pos.txt', 'rU').readlines())
  366.     return cnt
  367.  
  368.  
  369. if __name__ == '__main__':
  370.  
  371.     # load file
  372.     file = 'pos.txt'
  373.  
  374.     env = KukaReachEnv(is_render=True, is_good_view=False)
  375.  
  376.     for i in range(10):
  377.         loop = 1
  378.         obs = env.reset()
  379.         print(obs)
  380.         # TODO  add multi coordinates support
  381.         for j in range(1, getCount() + 1):
  382.             print(j)
  383.             action = np.array(getContent(j))
  384.             obs, reward, done, info, robot_obs = env.step(action)
  385.             print(colored("reward={},robot_obs={}".format(reward, robot_obs), "cyan"))
  386.             # print(colored("info={}".format(info),"cyan"))
  387.  
  388.             if done:
  389.                 break
  390.     print()
  391.  
  392.