Robot infra and env #7

Leo428 · 2024-01-11T03:38:48Z

In this PR, I added:

Functionalities to command robotiq gripper in robot servers
The base Franka + robotiq Gym environment
All the realsense camera supports
Relative coordinate transformation Gym env wrapper that enables all the cool results
peg insert, pcb insert, and cable routing Gym envs.

Soon Charles will add in a new cleaner spacemouse utils and wrappers.

Thank you for reviewing! Happy New Year!

Signed-off-by: youliang <[email protected]>

youliangtan

I made some code changes to this PR. Overall, this PR should be good to go. Robot env is usually very custom to individual setup, thus there is a limit on how this serl_robot_infra can be a generic library.

The best way for us is to document this, so that user knows how to create their own robot env, and then run it. User can optionally choose to utilize existing env_wrappers or utils in their custom robot env.

serl_robot_infra/README.md

serl_robot_infra/franka_env/envs/franka_robotiq_env.py

Leo428 · 2024-01-11T08:12:06Z

Thank you @youliangtan so much for the cleanups and suggestions! I will test the updated code on the robot tomorrow and make more improvements.

Signed-off-by: youliang <[email protected]>

youliangtan · 2024-01-11T18:10:42Z

I created a renamed edgeml, -> agentlace. Not sure if this is a better name 😅

Updated the name in this commit too: 3fa119d

serl_robot_infra/franka_env/spacemouse/spacemouse_expert.py

jianlanluo · 2024-01-12T22:09:03Z

serl_robot_infra/franka_env/envs/franka_pcb_insert.py

+        self.action_scale = (0.02, 0.2, 1)
+
+    def crop_image(self, image):
+        return image[90:390, 170:470, :]


crop_image() should be optional

jianlanluo · 2024-01-12T22:10:17Z

serl_robot_infra/franka_env/envs/franka_pcb_insert.py

+        self._TARGET_POSE = np.array(
+            [
+                0.5668657154487453,
+                0.002050321710641817,
+                0.05462772570641611,
+                # 0.9998817571282431,-0.010581843089284471,0.008902600824764906,0.0067260729646395475
+                3.1279511,
+                0.0176617,
+                0.0212859,
+            ]
+        )
+        self.resetpos[:3] = self._TARGET_POSE[:3]
+        self.resetpos[2] += 0.04
+        self.resetpos[3:] = euler_2_quat(self._TARGET_POSE[3:])
+
+        # Bouding box
+        self.xyz_bounding_box = gym.spaces.Box(
+            self._TARGET_POSE[:3]
+            - np.array([self.random_xy_range, self.random_xy_range, 0.005]),
+            self._TARGET_POSE[:3]
+            + np.array([self.random_xy_range, self.random_xy_range, 0.05]),
+            dtype=np.float32,
+        )
+        rpy_delta_range = np.array([0.05, 0.05, self.random_rz_range])
+        self.rpy_bounding_box = gym.spaces.Box(
+            self._TARGET_POSE[3:] - rpy_delta_range,
+            self._TARGET_POSE[3:] + rpy_delta_range,
+            dtype=np.float32,
+        )
+
+        self._REWARD_THRESHOLD = [0.005, 0.005, 0.001, 0.1, 0.1, 0.1]
+        self.action_scale = (0.02, 0.2, 1)


like i said in the PR comments, we should probably get another python file and use a ConfigDict to encompass all experiment-related parameters, when we launch our experiments, we should only need python --config xxx.py

jianlanluo · 2024-01-12T22:10:34Z

serl_robot_infra/franka_env/envs/franka_pcb_insert.py

+        # requests.post(self.url + "pcb_compliance_mode")
+        self.update_currpos()
+        reset_pose = copy.deepcopy(self.currpos)
+        reset_pose[2] += 0.03
+        self.interpolate_move(reset_pose, timeout=1.5)
+
+        # time.sleep(2)
+        requests.post(self.url + "precision_mode")
+        time.sleep(1)  # wait for mode switching
+
+        reset_pose = self.resetpos.copy()
+        self.interpolate_move(reset_pose, timeout=1)
+
+        # perform random reset
+        if self.randomreset:  # randomize reset position in xy plane
+            reset_pose[:2] += np.random.uniform(
+                -self.random_xy_range, self.random_xy_range, (2,)
+            )
+            euler_random = self._TARGET_POSE[3:].copy()
+            euler_random[-1] += np.random.uniform(
+                -self.random_rz_range, self.random_rz_range
+            )
+            reset_pose[3:] = euler_2_quat(euler_random)
+            self.interpolate_move(reset_pose, timeout=1)
+
+        if jpos:
+            requests.post(self.url + "precision_mode")
+            print("JOINT RESET")
+            requests.post(self.url + "jointreset")
+
+        requests.post(self.url + "pcb_compliance_mode")


jianlanluo · 2024-01-12T22:10:54Z

serl_robot_infra/franka_env/envs/franka_peg_insert.py

+            ]
+        )
+        self.resetpos[:3] = self._TARGET_POSE[:3]
+        self.resetpos[2] += 0.1
+        self.resetpos[3:] = euler_2_quat(self._TARGET_POSE[3:])
+
+        # Bouding box
+        self.xyz_bounding_box = gym.spaces.Box(
+            self._TARGET_POSE[:3]
+            - np.array([self.random_xy_range, self.random_xy_range, 0]),
+            self._TARGET_POSE[:3]
+            + np.array([self.random_xy_range, self.random_xy_range, 0.1]),
+            dtype=np.float32,
+        )
+        rpy_delta_range = np.array([0.01, 0.01, self.random_rz_range])
+        self.rpy_bounding_box = gym.spaces.Box(
+            self._TARGET_POSE[3:] - rpy_delta_range,
+            self._TARGET_POSE[3:] + rpy_delta_range,
+            dtype=np.float32,


abstract this away in config file

serl_robot_infra/franka_env/envs/relative_env.py

jianlanluo · 2024-01-16T00:15:47Z

Can someone address my comment on putting env/experiment related config into a separate config file?

then we can just python --config PATH_TO_CONFIG.py

Signed-off-by: youliang <[email protected]>

youliangtan · 2024-01-16T17:52:54Z

@Leo428 @charlesxu0124 In 752ded1. I briefly try to split out the config by having custom configs in both peg and pcb insertion examples:

examples/async_pcb_insert_drq/config.py and examples/async_peg_insert_drq/config.py.
we dont have an example of cable routing in the examples, so retain the config in the franka_cable_route.py for now
This is just the first iteration of organizing the config, most likely need another round of cleanup
did some cleans and fixes . definitely need to try test on robot

Signed-off-by: youliang <[email protected]>

youliangtan · 2024-01-18T04:38:26Z

serl_robot_infra/franka_env/envs/franka_peg_insert.py

+
+    def go_to_rest(self, jpos=False):
+        # TODO: Implement this
+        return NotImplementedError


I noticed that go_to_rest is not impl here added this TODO. Not sure if this is giving an issue.

Also I make the workspace boundary configurable in the config class. Best if we can test it on the robot to ensure everything still works fine.

Signed-off-by: youliang <[email protected]>

jianlanluo · 2024-01-21T02:45:25Z

@youliangtan @Leo428 @charlesxu0124

are the required config changes done?

youliangtan · 2024-01-21T02:52:56Z

Yes done. will need @Leo428 help to simply test it out the experiments to make sure things are not breaking.

jianlanluo · 2024-01-21T06:55:34Z

@Leo428 @charlesxu0124 can you test it tomorrow? then I believe we can close this PR

Leo428 · 2024-01-21T07:00:40Z

We are testing it right now along with BC baseline.

…fix.

Leo428 · 2024-01-21T08:13:47Z

We refactor, fixed some minor mistakes, and tested the env config files. They should be good to merge now.

Leo428 · 2024-01-21T08:15:21Z

Once we merge this one, I will add BC training eval scripts, pick and place task, and vice agent (if it works) in the next PR.

Leo428 · 2024-01-21T08:32:25Z

@youliangtan hi youliang, can you take a look and merge it? Thanks

youliangtan

All good! Thanks for the commit

Leo428 added 7 commits January 10, 2024 19:23

add gripper move controls to the robot servers

fbcf675

add basic franka + robotiq gripper gym env

43a1c84

add realsense camera supports as an example

bad53e3

add relative coordinate transformation gym wrapper and utils

e771baf

add peg insertion gym env, and useful gym env wrappers

e67332d

add pcb insert gym env example

d056f47

add cable route gym env example

bb8a9c0

Leo428 requested review from jianlanluo, youliangtan and charlesxu0124 January 11, 2024 03:39

charlesxu0124 and others added 4 commits January 10, 2024 19:59

changed over to pyspacemouse

4860e7b

fix robot_infra path

20df064

relocate spacemouse files

b20fadd

bunch of cleanups

fc47b42

Signed-off-by: youliang <[email protected]>

youliangtan suggested changes Jan 11, 2024

View reviewed changes

serl_robot_infra/README.md Show resolved Hide resolved

serl_robot_infra/README.md Show resolved Hide resolved

serl_robot_infra/franka_env/envs/franka_robotiq_env.py Outdated Show resolved Hide resolved

minor clean and rename edgeml to agentlace

3fa119d

Signed-off-by: youliang <[email protected]>

jianlanluo requested changes Jan 12, 2024

View reviewed changes

charlesxu0124 added 6 commits January 14, 2024 02:09

renamed enviornment since it is gripper agnostic

bb11575

clean up and edit comment

a5effed

applied pre-commit

ad15b5c

follow adjoint matrix notation in modern robotics

b46ec5d

subscribe to FT msg in stiffness frame

7f4fb4a

added comment for clipping func

89026fa

try clean configs

752ded1

Signed-off-by: youliang <[email protected]>

cleanups env, configurable pose limit in config, update readme

a876cdb

Signed-off-by: youliang <[email protected]>

youliangtan reviewed Jan 18, 2024

View reviewed changes

youliangtan and others added 2 commits January 18, 2024 12:39

nit

3258fcd

Signed-off-by: youliang <[email protected]>

Added robot control flowchart

5ca7c24

add go_to_reset for peg insert env

e2a5cc0

env config refactor for peg, pcb, cable. Minor franka server gripper …

4f17575

…fix.

jianlanluo approved these changes Jan 21, 2024

View reviewed changes

youliangtan approved these changes Jan 21, 2024

View reviewed changes

youliangtan merged commit 707d5f3 into main Jan 21, 2024
1 check passed

youliangtan deleted the robot_infra_and_env branch January 21, 2024 10:28

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Robot infra and env #7

Robot infra and env #7

Leo428 commented Jan 11, 2024

youliangtan left a comment •

edited

Loading

Leo428 commented Jan 11, 2024

youliangtan commented Jan 11, 2024

jianlanluo Jan 12, 2024

jianlanluo Jan 12, 2024

jianlanluo Jan 12, 2024

jianlanluo Jan 12, 2024

jianlanluo commented Jan 16, 2024

youliangtan commented Jan 16, 2024

youliangtan Jan 18, 2024 •

edited

Loading

jianlanluo commented Jan 21, 2024

youliangtan commented Jan 21, 2024

jianlanluo commented Jan 21, 2024

Leo428 commented Jan 21, 2024

Leo428 commented Jan 21, 2024

Leo428 commented Jan 21, 2024

Leo428 commented Jan 21, 2024

youliangtan left a comment

Robot infra and env #7

Robot infra and env #7

Conversation

Leo428 commented Jan 11, 2024

youliangtan left a comment • edited Loading

Choose a reason for hiding this comment

Leo428 commented Jan 11, 2024

youliangtan commented Jan 11, 2024

jianlanluo Jan 12, 2024

Choose a reason for hiding this comment

jianlanluo Jan 12, 2024

Choose a reason for hiding this comment

jianlanluo Jan 12, 2024

Choose a reason for hiding this comment

jianlanluo Jan 12, 2024

Choose a reason for hiding this comment

jianlanluo commented Jan 16, 2024

youliangtan commented Jan 16, 2024

youliangtan Jan 18, 2024 • edited Loading

Choose a reason for hiding this comment

jianlanluo commented Jan 21, 2024

youliangtan commented Jan 21, 2024

jianlanluo commented Jan 21, 2024

Leo428 commented Jan 21, 2024

Leo428 commented Jan 21, 2024

Leo428 commented Jan 21, 2024

Leo428 commented Jan 21, 2024

youliangtan left a comment

Choose a reason for hiding this comment

youliangtan left a comment •

edited

Loading

youliangtan Jan 18, 2024 •

edited

Loading