Skip to content

Commit 3c5f367

Browse files
kennkhrkennkhr
committed
feat: twin grasp
1 parent 146fa77 commit 3c5f367

10 files changed

Lines changed: 129 additions & 72 deletions

File tree

‎extensions/rcs_fr3/src/hw/FR3.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -113,7 +113,7 @@ class FR3 : public common::Robot {
113113
common::Pose get_base_pose_in_world_coordinates() override;
114114

115115
void reset() override;
116-
void close() override{};
116+
void close() override {};
117117
};
118118
} // namespace hw
119119
} // namespace rcs

‎extensions/rcs_fr3/src/hw/FrankaHand.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -76,7 +76,7 @@ class FrankaHand : public common::Gripper {
7676
void grasp() override;
7777
void open() override;
7878
void shut() override;
79-
void close() override{};
79+
void close() override {};
8080
};
8181
} // namespace hw
8282
} // namespace rcs

‎extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py‎

Lines changed: 63 additions & 50 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,15 @@
11
import logging
2-
from time import sleep
32
import math
3+
from time import sleep
44

55
import numpy as np
66
from rcs._core.common import RobotPlatform
77
from rcs.envs.base import ControlMode, RelativeTo
88
from rcs.envs.creators import SimEnvCreator
9-
from rcs.envs.utils import get_tcp_offset
10-
from rcs_xarm7.creators import RCSXArm7EnvCreator, XArm7SimEnvCreator
9+
from rcs.envs.utils import default_sim_tilburg_hand_cfg, get_tcp_offset
1110
from rcs.hand.tilburg_hand import THConfig
11+
from rcs_xarm7.creators import RCSXArm7EnvCreator, XArm7SimEnvCreator
12+
1213
import rcs
1314
from rcs import sim
1415

@@ -20,13 +21,53 @@
2021
ROBOT_INSTANCE = RobotPlatform.HARDWARE
2122

2223

24+
def sim_env():
25+
robot_cfg = sim.SimRobotConfig()
26+
robot_cfg.actuators = [
27+
"act1",
28+
"act2",
29+
"act3",
30+
"act4",
31+
"act5",
32+
"act6",
33+
"act7",
34+
]
35+
robot_cfg.joints = [
36+
"joint1",
37+
"joint2",
38+
"joint3",
39+
"joint4",
40+
"joint5",
41+
"joint6",
42+
"joint7",
43+
]
44+
robot_cfg.base = "base"
45+
robot_cfg.robot_type = rcs.common.RobotType.XArm7
46+
robot_cfg.attachment_site = "attachment_site"
47+
robot_cfg.arm_collision_geoms = []
48+
robot_cfg.tcp_offset = get_tcp_offset(rcs.scenes["xarm7_empty_world"]["mjcf_robot"])
49+
env_rel = SimEnvCreator()(
50+
control_mode=ControlMode.JOINTS,
51+
collision_guard=False,
52+
robot_cfg=robot_cfg,
53+
gripper_cfg=None,
54+
hand_cfg=default_sim_tilburg_hand_cfg(),
55+
# cameras=default_mujoco_cameraset_cfg(),
56+
# max_relative_movement=0.5,
57+
relative_to=RelativeTo.LAST_STEP,
58+
# mjcf=rcs.scenes["xarm7_empty_world"]["mjb"],
59+
robot_kinematics_path=rcs.scenes["xarm7_empty_world"]["mjcf_robot"],
60+
mjcf="/home/ken/robot-control-stack/models/scenes/xarm7_tilburg/scene.xml",
61+
)
62+
env_rel.get_wrapper_attr("sim").open_gui()
63+
return env_rel
64+
65+
2366
def main():
2467

2568
if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
2669
hand_cfg = THConfig(
27-
calibration_file="/home/ken/tilburg_hand/calibration.json",
28-
grasp_percentage=1,
29-
hand_orientation="right"
70+
calibration_file="/home/ken/tilburg_hand/calibration.json", grasp_percentage=1, hand_orientation="right"
3071
)
3172
env_rel = RCSXArm7EnvCreator()(
3273
control_mode=ControlMode.JOINTS,
@@ -36,73 +77,45 @@ def main():
3677
max_relative_movement=None,
3778
)
3879
else:
39-
robot_cfg = sim.SimRobotConfig()
40-
robot_cfg.actuators = [
41-
"act1",
42-
"act2",
43-
"act3",
44-
"act4",
45-
"act5",
46-
"act6",
47-
"act7",
48-
]
49-
robot_cfg.joints = [
50-
"joint1",
51-
"joint2",
52-
"joint3",
53-
"joint4",
54-
"joint5",
55-
"joint6",
56-
"joint7",
57-
]
58-
robot_cfg.base = "base"
59-
robot_cfg.robot_type = rcs.common.RobotType.XArm7
60-
robot_cfg.attachment_site = "attachment_site"
61-
robot_cfg.arm_collision_geoms = []
62-
robot_cfg.tcp_offset = get_tcp_offset(rcs.scenes["xarm7_empty_world"]["mjcf_robot"])
63-
env_rel = SimEnvCreator()(
64-
control_mode=ControlMode.CARTESIAN_TQuat,
65-
collision_guard=False,
66-
robot_cfg=robot_cfg,
67-
gripper_cfg=None,
68-
# cameras=default_mujoco_cameraset_cfg(),
69-
max_relative_movement=0.5,
70-
relative_to=RelativeTo.LAST_STEP,
71-
mjcf=rcs.scenes["xarm7_empty_world"]["mjb"],
72-
robot_kinematics_path=rcs.scenes["xarm7_empty_world"]["mjcf_robot"],
73-
)
74-
env_rel.get_wrapper_attr("sim").open_gui()
80+
env_rel = sim_env()
81+
82+
twin_env = sim_env()
83+
twin_robot = twin_env.unwrapped.robot
84+
twin_sim = twin_env.get_wrapper_attr("sim")
7585

7686
env_rel.reset()
7787

7888
actions = [
7989
# open hand
80-
( [0, math.radians(-45), 0, math.radians(15), 0, math.radians(-25), 0], 1, 2.0 ),
90+
([0, math.radians(-45), 0, math.radians(15), 0, math.radians(-25), 0], 1, 2.0),
8191
# approach
82-
( [0, math.radians(45), 0, math.radians(40), 0, math.radians(-95), 0], 1, 2.0 ),
92+
([0, math.radians(45), 0, math.radians(40), 0, math.radians(-95), 0], 1, 2.0),
8393
# close hand
84-
( [0, math.radians(45), 0, math.radians(40), 0, math.radians(-95), 0], 0, 2.0 ),
94+
([0, math.radians(45), 0, math.radians(40), 0, math.radians(-95), 0], 0, 2.0),
8595
# lift
86-
( [0, math.radians(15), 0, math.radians(30), 0, math.radians(-75), 0], 0, 4.0 ),
96+
([0, math.radians(15), 0, math.radians(30), 0, math.radians(-75), 0], 0, 4.0),
8797
# put back
88-
( [0, math.radians(45), 0, math.radians(40), 0, math.radians(-95), 0], 0, 2.0 ),
98+
([0, math.radians(45), 0, math.radians(40), 0, math.radians(-95), 0], 0, 2.0),
8999
# open hand
90-
( [0, math.radians(45), 0, math.radians(40), 0, math.radians(-95), 0], 1, 2.0 ),
100+
([0, math.radians(45), 0, math.radians(40), 0, math.radians(-95), 0], 1, 2.0),
91101
# back to home
92-
( [0, math.radians(-45), 0, math.radians(15), 0, math.radians(-25), 0], 1, 0.0 ),
102+
([0, math.radians(-45), 0, math.radians(15), 0, math.radians(-25), 0], 1, 0.0),
93103
]
94104

95105
with env_rel:
96106
for joints, hand, delay in actions:
97107
act = {"joints": joints, "hand": hand}
108+
twin_env.step(act)
98109
obs, reward, terminated, truncated, info = env_rel.step(act)
110+
# twin_robot.set_joint_position(joints)
111+
# twin_sim.step(50)
112+
sleep(1)
99113
if truncated or terminated:
100114
logger.info("Truncated or terminated!")
101115
break
102116
if delay > 0:
103117
sleep(delay)
104118

105119

106-
107120
if __name__ == "__main__":
108121
main()

‎include/rcs/Robot.h‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,14 @@ static const std::unordered_map<RobotType, RobotMetaConfig> robots_meta_config =
6060
// -------------- XArm7 --------------
6161
{XArm7, RobotMetaConfig{
6262
// q_home (7‐vector):
63-
(VectorXd(7) << 0, 0, 0, 0, 0, 0, 0).finished(),
63+
(VectorXd(7) << 0,
64+
-45. / 180. * M_PI,
65+
0,
66+
15. / 180. * M_PI,
67+
0,
68+
-25. / 180. * M_PI,
69+
0
70+
).finished(),
6471
// dof:
6572
7,
6673
// joint_limits (2×7):

‎python/rcs/envs/base.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -802,6 +802,6 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]:
802802
self._last_hand_cmd = hand_action
803803
del action[self.hand_key]
804804
return action
805-
805+
806806
def close(self):
807807
self._hand.close()

‎python/rcs/envs/sim.py‎

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,13 @@
33

44
import gymnasium as gym
55
import numpy as np
6-
from rcs.envs.base import ControlMode, GripperWrapper, HandWrapper, MultiRobotWrapper, RobotEnv
6+
from rcs.envs.base import (
7+
ControlMode,
8+
GripperWrapper,
9+
HandWrapper,
10+
MultiRobotWrapper,
11+
RobotEnv,
12+
)
713
from rcs.envs.space_utils import ActObsInfoWrapper
814
from rcs.envs.utils import default_sim_robot_cfg, default_sim_tilburg_hand_cfg
915

@@ -120,6 +126,8 @@ def __init__(self, env, hand: sim.SimTilburgHand):
120126
self._hand = hand
121127

122128
def action(self, action: dict[str, Any]) -> dict[str, Any]:
129+
if isinstance(action["hand"], int | float):
130+
return action
123131
if len(action["hand"]) == 18:
124132
action["hand"] = action["hand"][:16]
125133
assert len(action["hand"]) == 16 or len(action["hand"]) == 1, "Hand action must be of length 16 or 1"

‎python/rcs/hand/tilburg_hand.py‎

Lines changed: 43 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -52,21 +52,49 @@ class TilburgHand(common.Hand):
5252

5353
POWER_GRASP_VALUES = np.array(
5454
[
55-
0.5, 0.5, 0.5, 1.4, # THUMB_(IP, MCP, ABD, CMC)
56-
0.5, 0.5, 1.0, 0.7, # INDEX_(DIP, PIP, MCP, ABD)
57-
0.5, 0.5, 1.0, 0.3,
58-
0.5, 0.5, 1.0, 0.0,
59-
0, 0
60-
], dtype=np.float32
55+
0.5,
56+
0.5,
57+
0.5,
58+
1.4, # THUMB_(IP, MCP, ABD, CMC)
59+
0.5,
60+
0.5,
61+
1.0,
62+
0.7, # INDEX_(DIP, PIP, MCP, ABD)
63+
0.5,
64+
0.5,
65+
1.0,
66+
0.3,
67+
0.5,
68+
0.5,
69+
1.0,
70+
0.0,
71+
0,
72+
0,
73+
],
74+
dtype=np.float32,
6175
)
6276
OPEN_VALUES = np.array(
6377
[
64-
0.0, 0.0, 0.5, 1.4, # THUMB_(IP, MCP, ABD, CMC)
65-
0.2, 0.2, 0.2, 0.7, # INDEX_(DIP, PIP, MCP, ABD)
66-
0.2, 0.2, 0.2, 0.3,
67-
0.2, 0.2, 0.2, 0.0,
68-
0, 0
69-
], dtype=np.float32
78+
0.0,
79+
0.0,
80+
0.5,
81+
1.4, # THUMB_(IP, MCP, ABD, CMC)
82+
0.2,
83+
0.2,
84+
0.2,
85+
0.7, # INDEX_(DIP, PIP, MCP, ABD)
86+
0.2,
87+
0.2,
88+
0.2,
89+
0.3,
90+
0.2,
91+
0.2,
92+
0.2,
93+
0.0,
94+
0,
95+
0,
96+
],
97+
dtype=np.float32,
7098
)
7199

72100
def __init__(self, cfg: THConfig, verbose: bool = False):
@@ -103,8 +131,9 @@ def set_pos_vector(self, pos_vector: Vec18Type):
103131
"""
104132
Sets the position vector for the motors.
105133
"""
106-
assert len(pos_vector) == (self._motors.n_motors), \
107-
f"Invalid position vector length: {len(pos_vector)}. Expected: {self._motors.n_motors}"
134+
assert len(pos_vector) == (
135+
self._motors.n_motors
136+
), f"Invalid position vector length: {len(pos_vector)}. Expected: {self._motors.n_motors}"
108137
self._motors.set_pos_vector(copy.deepcopy(pos_vector), unit=self._cfg.control_unit)
109138

110139
def set_zero_pos(self):

‎src/sim/SimGripper.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -88,7 +88,7 @@ class SimGripper : public common::Gripper {
8888
void grasp() override;
8989
void open() override;
9090
void shut() override;
91-
void close() override{};
91+
void close() override {};
9292
};
9393
} // namespace sim
9494
} // namespace rcs

‎src/sim/SimRobot.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -76,7 +76,7 @@ class SimRobot : public common::Robot {
7676
std::optional<std::shared_ptr<common::IK>> get_ik() override;
7777
void reset() override;
7878
void set_joints_hard(const common::VectorXd &q);
79-
void close() override{};
79+
void close() override {};
8080

8181
private:
8282
SimRobotConfig cfg;

‎src/sim/SimTilburgHand.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -120,7 +120,7 @@ class SimTilburgHand : public common::Hand {
120120
void grasp() override;
121121
void open() override;
122122
void shut() override;
123-
void close() override{};
123+
void close() override {};
124124
};
125125
} // namespace sim
126126
} // namespace rcs

0 commit comments

Comments
 (0)