Skip to content

Commit b05c627

Browse files
Skylark0924Skylark0924
committed
🎮 Fix bugs
1 parent 6e5c512 commit b05c627

File tree

11 files changed

+109
-190
lines changed

11 files changed

+109
-190
lines changed

‎examples/simulator/example_curi_track_traj.py‎

Lines changed: 24 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -1,18 +1,24 @@
1-
"""
2-
Tracking the trajectory with multiple joints by CURI
3-
============================================================
4-
5-
This example runs a Tai Chi demo bimanual trajectory by using CURI.
6-
"""
7-
8-
import numpy as np
9-
10-
import rofunc as rf
11-
12-
traj_l = np.load('../data/LQT_LQR/taichi_1l.npy')
13-
traj_r = np.load('../data/LQT_LQR/taichi_1r.npy')
14-
rf.lqt.plot_3d_bi(traj_l, traj_r, ori=False)
15-
16-
args = rf.config.get_sim_config("CURI")
17-
CURIsim = rf.sim.CURISim(args)
18-
CURIsim.run_traj(traj=[traj_l, traj_r], update_freq=0.001)
1+
"""
2+
Tracking the trajectory with multiple joints by CURI
3+
============================================================
4+
5+
This example runs a Tai Chi demo bimanual trajectory by using CURI.
6+
"""
7+
8+
import numpy as np
9+
10+
import rofunc as rf
11+
12+
traj_l = np.load('../data/LQT_LQR/taichi_1l.npy')
13+
traj_r = np.load('../data/LQT_LQR/taichi_1r.npy')
14+
traj_l[:, 0] += 0.2
15+
traj_r[:, 0] += 0.2
16+
traj_l[:, 1] = -traj_l[:, 1]
17+
traj_r[:, 1] = -traj_r[:, 1]
18+
traj_l[:, 3:] = [0.707, 0., 0, 0.707]
19+
traj_r[:, 3:] = [-0., 0.707, 0.707, -0.]
20+
rf.lqt.plot_3d_bi(traj_l, traj_r, ori=False)
21+
22+
args = rf.config.get_sim_config("CURI")
23+
CURIsim = rf.sim.CURISim(args)
24+
CURIsim.run_traj(traj=[traj_l, traj_r], update_freq=0.001)

‎examples/simulator/example_franka_track_traj.py‎

Lines changed: 21 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -1,17 +1,21 @@
1-
"""
2-
Tracking the trajectory by Franka
3-
============================================================
4-
5-
This example runs a Tai Chi demo trajectory by using Franka.
6-
"""
7-
8-
import numpy as np
9-
10-
import rofunc as rf
11-
12-
traj = np.load('../data/LQT_LQR/taichi_1l.npy')
13-
rf.lqt.plot_3d_uni(traj, ori=False)
14-
15-
args = rf.config.get_sim_config("Franka")
16-
frankasim = rf.sim.FrankaSim(args)
17-
frankasim.run_traj(traj)
1+
"""
2+
Tracking the trajectory by Franka
3+
============================================================
4+
5+
This example runs a Tai Chi demo trajectory by using Franka.
6+
"""
7+
8+
import numpy as np
9+
10+
import rofunc as rf
11+
12+
traj = np.load('../data/LQT_LQR/taichi_1l.npy')
13+
traj[:, 0] -= 0.5
14+
traj[:, 1] += 0.2
15+
traj[:, 2] -= 0.6
16+
traj[:, 3:] = [0., 1, 0, 0]
17+
rf.lqt.plot_3d_uni(traj, ori=False)
18+
19+
args = rf.config.get_sim_config("Franka")
20+
frankasim = rf.sim.FrankaSim(args)
21+
frankasim.run_traj(traj)

‎examples/simulator/example_robot_show.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@
1010
# CURI
1111
# args = rf.config.get_sim_config("CURI")
1212
# CURIsim = rf.sim.CURISim(args)
13-
# CURIsim.show(visual_obs_flag=True)
13+
# CURIsim.show()
1414

1515
# walker
1616
# args = rf.config.get_sim_config("Walker")
@@ -42,7 +42,7 @@
4242
# Gluonsim = rf.sim.GluonSim(args)
4343
# Gluonsim.show()
4444

45-
# Multi Robots
45+
# TODO: Multi Robots
4646
# curi_args = rf.config.get_sim_config("CURI")
4747
# CURIsim = rf.sim.CURISim(curi_args)
4848
# walker_args = rf.config.get_sim_config("Walker")

‎rofunc/config/simulator/Baxter.yaml‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,8 @@ env:
1212
robot_name: "Baxter"
1313
assetRoot:
1414
assetFile: "urdf/baxter/robot.xml"
15-
init_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
16-
fix_base_link: True
15+
init_pose: [0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
16+
fix_base_link: False
1717
flip_visual_attachments: True
1818
armature: 0.01
1919

‎rofunc/config/simulator/Sawyer.yaml‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,8 @@ env:
1212
robot_name: "Sawyer"
1313
assetRoot:
1414
assetFile: "urdf/sawyer/robot_torque.xml"
15-
init_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
16-
fix_base_link: True
15+
init_pose: [0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
16+
fix_base_link: False
1717
flip_visual_attachments: True
1818
armature: 0.01
1919

‎rofunc/simulator/base_sim.py‎

Lines changed: 43 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -151,7 +151,7 @@ def create_env(self):
151151
asset_options.armature = self.args.env.asset.armature
152152

153153
init_pose = self.args.env.asset.init_pose
154-
robot_name = self.args.env.asset.robot_name
154+
self.robot_name = self.args.env.asset.robot_name
155155

156156
beauty_print("Loading robot asset {} from {}".format(asset_file, asset_root), type="info")
157157
self.robot_asset = self.gym.load_asset(self.sim, asset_root, asset_file, asset_options)
@@ -176,17 +176,55 @@ def create_env(self):
176176
envs.append(env)
177177

178178
# add robot
179-
robot_handle = self.gym.create_actor(env, self.robot_asset, pose, robot_name, i, 2)
179+
robot_handle = self.gym.create_actor(env, self.robot_asset, pose, self.robot_name, i, 2)
180180
self.gym.enable_actor_dof_force_sensors(env, robot_handle)
181181
robot_handles.append(robot_handle)
182182

183183
self.envs = envs
184184
self.robot_handles = robot_handles
185185

186-
def setup_robot_dof_prop(self, **kwargs):
187-
raise NotImplementedError
186+
def setup_robot_dof_prop(self):
187+
from isaacgym import gymapi
188+
189+
gym = self.gym
190+
envs = self.envs
191+
robot_asset = self.robot_asset
192+
robot_handles = self.robot_handles
193+
194+
# configure robot dofs
195+
robot_dof_props = gym.get_asset_dof_properties(robot_asset)
196+
robot_lower_limits = robot_dof_props["lower"]
197+
robot_upper_limits = robot_dof_props["upper"]
198+
robot_ranges = robot_upper_limits - robot_lower_limits
199+
robot_mids = 0.3 * (robot_upper_limits + robot_lower_limits)
200+
201+
robot_dof_props["driveMode"][:].fill(gymapi.DOF_MODE_POS)
202+
robot_dof_props["stiffness"][:].fill(300.0)
203+
robot_dof_props["damping"][:].fill(30.0)
204+
205+
# default dof states and position targets
206+
robot_num_dofs = gym.get_asset_dof_count(robot_asset)
207+
default_dof_pos = np.zeros(robot_num_dofs, dtype=np.float32)
208+
default_dof_pos[:] = robot_mids[:]
209+
210+
default_dof_state = np.zeros(robot_num_dofs, gymapi.DofState.dtype)
211+
default_dof_state["pos"] = default_dof_pos
212+
213+
# # send to torch
214+
# default_dof_pos_tensor = to_torch(default_dof_pos, device=device)
215+
216+
for env, robot_handle in zip(envs, robot_handles):
217+
# set dof properties
218+
gym.set_actor_dof_properties(env, robot_handle, robot_dof_props)
219+
220+
# set initial dof states
221+
gym.set_actor_dof_states(env, robot_handle, default_dof_state, gymapi.STATE_ALL)
222+
223+
# set initial position targets
224+
gym.set_actor_dof_position_targets(env, robot_handle, default_dof_pos)
188225

189226
def add_object(self):
227+
# TODO
190228
from isaacgym import gymapi
191229

192230
asset_root = self.args.env.object_asset.assetRoot or os.path.join(rf.oslab.get_rofunc_path(),
@@ -323,7 +361,7 @@ def show(self, visual_obs_flag=False, camera_props=None, attached_body=None, loc
323361
"""
324362
from isaacgym import gymapi
325363

326-
beauty_print("Show the {} simulator in the interactive mode".format(self.robot_name), 1)
364+
beauty_print("Show the {} simulator in the interactive mode".format(self.robot_name), type="module")
327365

328366
if visual_obs_flag:
329367
fig = plt.figure("Visual observation", figsize=(8, 8))

‎rofunc/simulator/baxter_sim.py‎

Lines changed: 0 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -20,43 +20,3 @@
2020
class BaxterSim(RobotSim):
2121
def __init__(self, args):
2222
super().__init__(args)
23-
24-
def setup_robot_dof_prop(self):
25-
from isaacgym import gymapi
26-
27-
gym = self.gym
28-
envs = self.envs
29-
robot_asset = self.robot_asset
30-
robot_handles = self.robot_handles
31-
32-
# configure robot dofs
33-
robot_dof_props = gym.get_asset_dof_properties(robot_asset)
34-
robot_lower_limits = robot_dof_props["lower"]
35-
robot_upper_limits = robot_dof_props["upper"]
36-
robot_ranges = robot_upper_limits - robot_lower_limits
37-
robot_mids = 0.3 * (robot_upper_limits + robot_lower_limits)
38-
39-
robot_dof_props["driveMode"][:].fill(gymapi.DOF_MODE_POS)
40-
robot_dof_props["stiffness"][:].fill(300.0)
41-
robot_dof_props["damping"][:].fill(30.0)
42-
43-
# default dof states and position targets
44-
robot_num_dofs = gym.get_asset_dof_count(robot_asset)
45-
default_dof_pos = np.zeros(robot_num_dofs, dtype=np.float32)
46-
default_dof_pos[:] = robot_mids[:]
47-
48-
default_dof_state = np.zeros(robot_num_dofs, gymapi.DofState.dtype)
49-
default_dof_state["pos"] = default_dof_pos
50-
51-
# # send to torch
52-
# default_dof_pos_tensor = to_torch(default_dof_pos, device=device)
53-
54-
for env, robot_handle in zip(envs, robot_handles):
55-
# set dof properties
56-
gym.set_actor_dof_properties(env, robot_handle, robot_dof_props)
57-
58-
# set initial dof states
59-
gym.set_actor_dof_states(env, robot_handle, default_dof_state, gymapi.STATE_ALL)
60-
61-
# set initial position targets
62-
gym.set_actor_dof_position_targets(env, robot_handle, default_dof_pos)

‎rofunc/simulator/curimini_sim.py‎

Lines changed: 2 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -16,9 +16,5 @@
1616

1717

1818
class CURIminiSim(RobotSim):
19-
def __init__(self, args, robot_name, asset_root=None, asset_file=None, fix_base_link=None,
20-
flip_visual_attachments=True, init_pose_vec=None, num_envs=1, device="cpu"):
21-
super().__init__(args, robot_name, asset_root, asset_file, fix_base_link, flip_visual_attachments,
22-
init_pose_vec, num_envs, device)
23-
self.asset_file = "urdf/curi_mini/urdf/diablo_simulation.urdf"
24-
self.flip_visual_attachments = False
19+
def __init__(self, args):
20+
super().__init__(args)

‎rofunc/simulator/franka_sim.py‎

Lines changed: 11 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -20,68 +20,27 @@ class FrankaSim(RobotSim):
2020
def __init__(self, args):
2121
super().__init__(args)
2222

23-
def setup_robot_dof_prop(
24-
self, gym=None, envs=None, robot_asset=None, robot_handles=None
25-
):
26-
from isaacgym import gymapi
27-
28-
gym = self.gym if gym is None else gym
29-
envs = self.envs if envs is None else envs
30-
robot_asset = self.robot_asset if robot_asset is None else robot_asset
31-
robot_handles = self.robot_handles if robot_handles is None else robot_handles
32-
33-
# configure robot dofs
34-
robot_dof_props = gym.get_asset_dof_properties(robot_asset)
35-
robot_lower_limits = robot_dof_props["lower"]
36-
robot_upper_limits = robot_dof_props["upper"]
37-
robot_ranges = robot_upper_limits - robot_lower_limits
38-
robot_mids = 0.3 * (robot_upper_limits + robot_lower_limits)
39-
40-
# default dof states and position targets
41-
robot_num_dofs = gym.get_asset_dof_count(robot_asset)
42-
default_dof_pos = np.zeros(robot_num_dofs, dtype=np.float32)
43-
default_dof_pos = robot_mids
44-
45-
default_dof_state = np.zeros(robot_num_dofs, gymapi.DofState.dtype)
46-
default_dof_state["pos"] = default_dof_pos
47-
48-
# # send to torch
49-
# default_dof_pos_tensor = to_torch(default_dof_pos, device=device)
50-
51-
for env, robot in zip(envs, robot_handles):
52-
# set dof properties
53-
gym.set_actor_dof_properties(env, robot, robot_dof_props)
54-
55-
# set initial dof states
56-
gym.set_actor_dof_states(env, robot, default_dof_state, gymapi.STATE_ALL)
57-
58-
# set initial position targets
59-
gym.set_actor_dof_position_targets(env, robot, default_dof_pos)
60-
61-
def update_robot(self, traj, attractor_handles, axes_geom, sphere_geom, index):
62-
# TODO: the traj is a little weird, need to be fixed
23+
def update_robot(self, traj, attractor_handles, axes_geom, sphere_geom, index, verbose=True):
6324
from isaacgym import gymutil
6425

65-
self.gym.clear_lines(self.viewer)
6626
for i in range(self.num_envs):
6727
# Update attractor target from current franka state
68-
attractor_properties = self.gym.get_attractor_properties(
69-
self.envs[i], attractor_handles[i]
70-
)
28+
attractor_properties = self.gym.get_attractor_properties(self.envs[i], attractor_handles[i])
7129
pose = attractor_properties.target
7230
# pose.p: (x, y, z), pose.r: (w, x, y, z)
73-
pose.p.x = traj[index, 0] * 0.5
74-
pose.p.y = traj[index, 2] * 0.5
75-
pose.p.z = traj[index, 1] * 0.5
31+
pose.p.x = traj[index, 0]
32+
pose.p.y = traj[index, 1]
33+
pose.p.z = traj[index, 2]
7634
pose.r.w = traj[index, 6]
7735
pose.r.x = traj[index, 3]
78-
pose.r.y = traj[index, 5]
79-
pose.r.z = traj[index, 4]
36+
pose.r.y = traj[index, 4]
37+
pose.r.z = traj[index, 5]
8038
self.gym.set_attractor_target(self.envs[i], attractor_handles[i], pose)
8139

82-
# Draw axes and sphere at attractor location
83-
gymutil.draw_lines(axes_geom, self.gym, self.viewer, self.envs[i], pose)
84-
gymutil.draw_lines(sphere_geom, self.gym, self.viewer, self.envs[i], pose)
40+
if verbose:
41+
# Draw axes and sphere at attractor location
42+
gymutil.draw_lines(axes_geom, self.gym, self.viewer, self.envs[i], pose)
43+
gymutil.draw_lines(sphere_geom, self.gym, self.viewer, self.envs[i], pose)
8544

8645
def run_traj(self, traj, attracted_joint="panda_hand"):
8746
self.run_traj_multi_joints([traj], [attracted_joint])

‎rofunc/simulator/sawyer_sim.py‎

Lines changed: 2 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -16,9 +16,5 @@
1616

1717

1818
class SawyerSim(RobotSim):
19-
def __init__(self, args, robot_name, asset_root=None, asset_file=None, fix_base_link=None,
20-
flip_visual_attachments=True, init_pose_vec=None, num_envs=1, device="cpu"):
21-
super().__init__(args, robot_name, asset_root, asset_file, fix_base_link, flip_visual_attachments,
22-
init_pose_vec, num_envs, device)
23-
self.asset_file = "urdf/sawyer/robot.xml"
24-
19+
def __init__(self, args):
20+
super().__init__(args)

0 commit comments

Comments
 (0)