RL-RVOp/simulator_cpp.py at master · XinyuWuu/RL-RVOp · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
import importlib
import videoIO
import matplotlib.pyplot as plt
import canvas
from CppClass.RVOcalculator import RVOcalculator
from CppClass.Observator import Observator
import contourGenerator
import envCreator
import render
import ctrlConverter
import matplotlib.pyplot as plt
from time import sleep
import numpy as np
from numpy import array, arctan2, flipud, zeros
from numpy.linalg import norm
import mujoco as mj
import reward
from CppClass.CtrlConverter import CtrlConverter

importlib.reload(envCreator)
importlib.reload(contourGenerator)
# importlib.reload(RVOcalculator)
importlib.reload(canvas)
importlib.reload(render)
importlib.reload(videoIO)
importlib.reload(reward)
importlib.reload(ctrlConverter)


class Simulator():

    def __init__(self, robot_r=0.17, dmax=4.0, framerate=50, dreach=0.02, avevel=True):
        self.robot_r = robot_r
        self.dmax = dmax
        self.dreach = dreach
        self.avevel = avevel
        self.EC = envCreator.EnvCreator()
        self.CG = contourGenerator.ContourGenrator(self.robot_r)
        self.OBS = Observator(self.dmax, self.robot_r)
        self.framerate = framerate
        self.step_time = 1 / framerate
        self.step_num = int(round(self.step_time / 0.002))
        self.step_time = self.step_num * 0.002
        self.set_reward()

    def set_reward(self, vmax=1.0, rmax=3.0, tolerance=0.005,
                   a=2.0, b=1.0, c=10.0,
                   d=1.0, e=1.0, f=20.0,
                   g=1.0, eta=0.5,
                   h=0.5, mu=0.75, rreach=30, remix=True, rm_middle=5, dmax=3, w=5, tb=0):
        self.OBS.set_reward(robot_r=self.robot_r, vmax=vmax, rmax=rmax, tolerance=tolerance,
                            a=a, b=b, c=c, d=d, e=e, f=f, g=g, eta=eta, h=h, mu=mu, remix=remix, rm_middle=rm_middle, dmax=dmax, w=w, tb=tb)
        self.rreach = rreach

    def set_model(self, Nrobot=0, robot_text="", obs_text="", obs=[], target_type='circle', offwidth: int = 1920, offheight: int = 1080, camheight: int = 15, fovy: int = 45):
        self.Nrobot = Nrobot
        actuator_text = self.EC.actuator(self.Nrobot)
        text = self.EC.env_text(robot_text, obs_text,
                                actuator_text, offwidth, offheight, camheight)
        self.obs = obs
        self.contours = self.CG.generate_contour(obs)

        self.mjMODEL = mj.MjModel.from_xml_string(text)
        self.mjDATA = mj.MjData(self.mjMODEL)

        self.ctrl = self.mjDATA.ctrl
        self.qpos = [self.mjDATA.joint(
            f"robot_{id}").qpos for id in range(self.Nrobot)]
        self.qvel = [self.mjDATA.joint(
            f"robot_{id}").qvel for id in range(self.Nrobot)]

        if target_type == "random":
            pass
        elif target_type == "spin":
            self.target = np.matmul(
                array(self.qpos)[:, 0:2], array([[0, 1.0], [-1.0, 0]]))
        elif target_type == "line":
            self.target = array(self.qpos)[:, 0:2] * array([-1.0, 1.0])
        else:  # "circle"
            self.target = -array(self.qpos)[:, 0:2]

        self.OBS.set_model(self.contours, self.target)
        self.pos_vel = zeros((self.Nrobot, 6))
        self.d = np.zeros((self.Nrobot))
        self.dpre = self.d.copy()
        return self.step(np.zeros((2 * self.Nrobot,)))

    def step(self, ctrl: np.ndarray, observe=True, isvs=False, CCcpp=CtrlConverter(1, 0.5)):
        # TODO save action to self.OBS
        if not isvs:
            for Nth in range(self.Nrobot):
                if self.d[Nth] == 1:
                    ctrl[Nth * 2: Nth * 2 + 2] = [0, 0]
            self.mjDATA.ctrl = ctrl
            mj.mj_step(self.mjMODEL, self.mjDATA, self.step_num)
            for j in range(self.Nrobot):
                self.pos_vel[j] = array([self.qpos[j][0], self.qpos[j][1],
                                        arctan2(
                                        2 * self.qpos[j][3] * self.qpos[j][6], 1 - 2 * self.qpos[j][6]**2),
                    self.qvel[j][0], self.qvel[j][1], self.qvel[j][5]], dtype=np.float32)
        else:
            _ctrl = ctrl.copy()
            for Nth in range(self.Nrobot):
                if self.d[Nth] == 1:
                    _ctrl[Nth] = [0, 0]
            if self.step_num % 5 == 0:
                step_num_sub = 5
            elif self.step_num % 6 == 0:
                step_num_sub = 6
            else:
                print("step error!!!!!!!!!!!!!!!!!!")
                exit(0)
            for t_step in range(int(self.step_num / step_num_sub)):
                self.mjDATA.ctrl = CCcpp.v2ctrlbatchL(
                    posvels=self.pos_vel, vs=_ctrl)
                mj.mj_step(self.mjMODEL, self.mjDATA, step_num_sub)
                for j in range(self.Nrobot):
                    self.pos_vel[j] = array([self.qpos[j][0], self.qpos[j][1],
                                            arctan2(
                                            2 * self.qpos[j][3] * self.qpos[j][6], 1 - 2 * self.qpos[j][6]**2),
                        self.qvel[j][0], self.qvel[j][1], self.qvel[j][5]], dtype=np.float32)

        observation = []
        if not observe:
            return array([]), [], array([]), [], array([]), array([])

        self.dpre = self.d.copy()
        self.d = array([1 if norm(self.target[Nth] - self.pos_vel[Nth][0:2]) <
                        self.dreach else 0
                        for Nth in range(self.Nrobot)])
        self.d = array([1 if self.dpre[i] == 1 or self.d[i] ==
                        1 else 0 for i in range(self.Nrobot)])

        # TODO get action history
        observation, r, NNinput = self.OBS.get_obs(
            self.pos_vel, self.avevel)

        r = array([r[rNth] + self.rreach if self.d[rNth] == 1 and self.dpre[rNth] ==
                   0 else r[rNth] for rNth in range(self.Nrobot)], dtype=np.float32)
        r = array([0 if self.dpre[rNth] == 1 else r[rNth]
                  for rNth in range(self.Nrobot)], dtype=np.float32)

        return self.pos_vel, observation, r, NNinput, self.d, self.dpre