robograsp/user_control.py at main · Z5cc/robograsp · 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
import pybullet as p
import time

from constants import CONE_CENTER
from environment.robot import Robot
from environment.obj import Obj
from environment.env import Env


def read_debug_parameter(dx_in, dy_in, dz_in, droll_in, dpitch_in, dyaw_in, gr_in):
    # read the value of task parameter
    dx = p.readUserDebugParameter(dx_in)
    dy = p.readUserDebugParameter(dy_in)
    dz = p.readUserDebugParameter(dz_in)
    droll = p.readUserDebugParameter(droll_in)
    dpitch = p.readUserDebugParameter(dpitch_in)
    dyaw = p.readUserDebugParameter(dyaw_in)
    gr = p.readUserDebugParameter(gr_in)
    return dx, dy, dz, droll, dpitch, dyaw, gr


env = Env()
env.reset_with_params(tcp_center=CONE_CENTER,tcp_target=(0,0,0),
                      obj_pos=(0,0,0),obj_orn=p.getQuaternionFromEuler((0,0,0)),obj_index=1)
# custom sliders to tune parameters (name of the parameter,range,initial value)
dx_in = p.addUserDebugParameter("dx", -0.01, 0.01, 0)
dy_in = p.addUserDebugParameter("dy", -0.01, 0.01, 0)
dz_in = p.addUserDebugParameter("dz", -0.01, 0.01, 0)
droll_in = p.addUserDebugParameter("droll", -0.2, 0.2, 0)
dpitch_in = p.addUserDebugParameter("dpitch", -0.2, 0.2, 0)
dyaw_in = p.addUserDebugParameter("dyaw", -0.2, 0.2, 0)
gr_in = p.addUserDebugParameter("gripper_opening_length", 0, 0.085, 0.04)

while True:
    debug_parameter = read_debug_parameter(dx_in, dy_in, dz_in, droll_in, dpitch_in, dyaw_in, gr_in)
    delta, gr_delta = debug_parameter[0:6], debug_parameter[-1]
    obs = env.step_user_control(delta,gr_delta)
    # lo, hi = p.getAABB(obj.id)
    # print(f'lo:{lo}')
    # print(f'hi:{hi}')
    # print(f'obs:{obs}')
    # print(f'shoulder_torque:{env.robot.get_shoulder_torque()}')
    # print(f'gripper_torque:{env.robot.gripper._get_torque()}')
    # print(f'gripper_angle:{env.robot.gripper._get_angle()}')
    # print(f'gripper_opening_length:{env.robot.gripper._get_opening_length()}')
    # print(f'ray_offest:{env.reward.ray_offset()}')