Skip to content

Commit 438fb97

Browse files
juelgjuelg
committed
refactor: compatility with current master
- fixed rl and pin iks - added forward function for iks - fixed stubfiles
1 parent b2f70c9 commit 438fb97

9 files changed

Lines changed: 31 additions & 28 deletions

File tree

‎python/examples/fr3.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,7 @@ def main():
6060
if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
6161
simulation = sim.Sim(rcs.scenes["fr3_empty_world"]["mjb"])
6262
urdf_path = rcs.scenes["fr3_empty_world"]["urdf"]
63-
ik = rcs.common.IK(str(urdf_path))
63+
ik = rcs.common.RL(str(urdf_path))
6464
cfg = sim.SimRobotConfig()
6565
cfg.add_id("0")
6666
cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
@@ -92,7 +92,7 @@ def main():
9292

9393
else:
9494
urdf_path = rcs.scenes["fr3_empty_world"]["urdf"]
95-
ik = rcs.common.IK(str(urdf_path))
95+
ik = rcs.common.RL(str(urdf_path))
9696
robot = rcs.hw.FR3(ROBOT_IP, ik)
9797
robot_cfg = FR3Config()
9898
robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())

‎python/rcs/_core/common.pyi‎

Lines changed: 0 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -21,12 +21,9 @@ __all__ = [
2121
"IdentityRotMatrix",
2222
"IdentityRotQuatVec",
2323
"IdentityTranslation",
24-
"NRobotsWithGripper",
2524
"Pin",
2625
"Pose",
27-
"RConfig",
2826
"RL",
29-
"Pose",
3027
"RPY",
3128
"Robot",
3229
"RobotConfig",
@@ -67,7 +64,6 @@ class GripperState:
6764
pass
6865

6966
class IK:
70-
def __init__(self, urdf_path: str, max_duration_ms: int = 300) -> None: ...
7167
def forward(self, q0: numpy.ndarray[M, numpy.dtype[numpy.float64]], tcp_offset: Pose = ...) -> Pose: ...
7268
def ik(
7369
self, pose: Pose, q0: numpy.ndarray[M, numpy.dtype[numpy.float64]], tcp_offset: Pose = ...
@@ -132,9 +128,6 @@ class Pose:
132128
def translation(self) -> numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]: ...
133129
def xyzrpy(self) -> numpy.ndarray[typing.Literal[6], numpy.dtype[numpy.float64]]: ...
134130

135-
class RConfig:
136-
pass
137-
138131
class RL(IK):
139132
def __init__(self, urdf_path: str, max_duration_ms: int = 300) -> None: ...
140133

‎python/rcs/envs/creators.py‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -84,7 +84,7 @@ def __call__( # type: ignore
8484
"""
8585
if urdf_path is None:
8686
urdf_path = rcs.scenes["fr3_empty_world"]["urdf"]
87-
ik = rcs.common.IK(str(urdf_path)) if urdf_path is not None else None
87+
ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None
8888
robot = rcs.hw.FR3(ip, ik)
8989
robot.set_parameters(robot_cfg)
9090

@@ -136,7 +136,7 @@ def __call__( # type: ignore
136136
) -> gym.Env:
137137

138138
urdf_path = rcs.scenes["fr3_empty_world"]["urdf"]
139-
ik = rcs.common.IK(str(urdf_path)) if urdf_path is not None else None
139+
ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None
140140
robots: dict[str, rcs.hw.FR3] = {}
141141
for ip in ips:
142142
robots[ip] = rcs.hw.FR3(ip, ik)
@@ -232,7 +232,7 @@ def __call__( # type: ignore
232232
mjcf = rcs.scenes[mjcf]["mjb"]
233233
simulation = sim.Sim(mjcf)
234234

235-
ik = rcs.common.IK(str(urdf_path))
235+
ik = rcs.common.RL(str(urdf_path))
236236
robot = rcs.sim.SimRobot(simulation, ik, robot_cfg)
237237
env: gym.Env = RobotEnv(robot, control_mode)
238238
env = RobotSimWrapper(env, simulation, sim_wrapper)

‎python/rcs_so101/creators.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -118,7 +118,7 @@ def __call__( # type: ignore
118118
"""
119119
simulation = sim.Sim(mjcf)
120120

121-
ik = rcs.common.IK(str(urdf_path))
121+
ik = rcs.common.RL(str(urdf_path))
122122
robot = rcs.sim.SimRobot(simulation, ik, robot_cfg)
123123
env: gym.Env = RobotEnv(robot, control_mode)
124124
env = RobotSimWrapper(env, simulation, sim_wrapper)

‎python/rcs_so101/hw.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@
77

88
class SO101(common.Robot):
99
def __init__(self, hf_robot: SO101Follower, urdf_path: str):
10-
self.ik = common.IK(urdf_path=urdf_path)
10+
self.ik = common.RL(urdf_path=urdf_path)
1111
self._hf_robot = hf_robot
1212
self._hf_robot.connect()
1313

‎src/common/IK.cpp‎

Lines changed: 17 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@ RL::RL(const std::string& urdf_path, size_t max_duration_ms) : rl_data() {
3232
this->rl_data.ik->setDuration(std::chrono::milliseconds(max_duration_ms));
3333
}
3434

35-
std::optional<VectorXd> IK::ik(const Pose& pose, const VectorXd& q0,
35+
std::optional<VectorXd> RL::ik(const Pose& pose, const VectorXd& q0,
3636
const Pose& tcp_offset) {
3737
// pose is assumed to be in the robots coordinate frame
3838
this->rl_data.kin->setPosition(q0);
@@ -50,6 +50,14 @@ std::optional<VectorXd> IK::ik(const Pose& pose, const VectorXd& q0,
5050
return std::nullopt;
5151
}
5252
}
53+
Pose RL::forward(const VectorXd& q0, const Pose& tcp_offset) {
54+
// pose is assumed to be in the robots coordinate frame
55+
this->rl_data.kin->setPosition(q0);
56+
this->rl_data.kin->forwardPosition();
57+
rcs::common::Pose pose = this->rl_data.kin->getOperationalPosition(0);
58+
// apply the tcp offset
59+
return pose * tcp_offset.inverse();
60+
}
5361

5462
Pin::Pin(const std::string& urdf_path, const std::string& frame_id) : model() {
5563
pinocchio::urdf::buildModel(urdf_path, this->model);
@@ -61,10 +69,10 @@ Pin::Pin(const std::string& urdf_path, const std::string& frame_id) : model() {
6169
}
6270
}
6371

64-
std::optional<Vector7d> Pin::ik(const Pose& pose, const Vector7d& q0,
72+
std::optional<VectorXd> Pin::ik(const Pose& pose, const VectorXd& q0,
6573
const Pose& tcp_offset) {
6674
rcs::common::Pose new_pose = pose * tcp_offset.inverse();
67-
Vector7d q(q0);
75+
VectorXd q(q0);
6876
const pinocchio::SE3 oMdes(new_pose.rotation_m(), new_pose.translation());
6977
pinocchio::Data::Matrix6x J(6, model.nv);
7078
J.setZero();
@@ -101,11 +109,13 @@ std::optional<Vector7d> Pin::ik(const Pose& pose, const Vector7d& q0,
101109
}
102110
}
103111

104-
Pose IK::forward(const VectorXd& q0, const Pose& tcp_offset) {
112+
Pose Pin::forward(const VectorXd& q0, const Pose& tcp_offset) {
105113
// pose is assumed to be in the robots coordinate frame
106-
this->rl.kin->setPosition(q0);
107-
this->rl.kin->forwardPosition();
108-
rcs::common::Pose pose = this->rl.kin->getOperationalPosition(0);
114+
pinocchio::forwardKinematics(model, data, q0);
115+
pinocchio::updateFramePlacements(model, data);
116+
rcs::common::Pose pose(data.oMf[this->FRAME_ID].rotation(),
117+
data.oMf[this->FRAME_ID].translation());
118+
109119
// apply the tcp offset
110120
return pose * tcp_offset.inverse();
111121
}

‎src/common/IK.h‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@ class IK {
2626
virtual std::optional<VectorXd> ik(
2727
const Pose& pose, const VectorXd& q0,
2828
const Pose& tcp_offset = Pose::Identity()) = 0;
29+
virtual Pose forward(const VectorXd& q0, const Pose& tcp_offset) = 0;
2930
};
3031

3132
class RL : public IK {
@@ -43,6 +44,7 @@ class RL : public IK {
4344
std::optional<VectorXd> ik(
4445
const Pose& pose, const VectorXd& q0,
4546
const Pose& tcp_offset = Pose::Identity()) override;
47+
Pose forward(const VectorXd& q0, const Pose& tcp_offset) override;
4648
};
4749

4850
class Pin : public IK {
@@ -58,9 +60,10 @@ class Pin : public IK {
5860

5961
public:
6062
Pin(const std::string& urdf_path, const std::string& frame_id);
61-
std::optional<Vector7d> ik(
62-
const Pose& pose, const Vector7d& q0,
63+
std::optional<VectorXd> ik(
64+
const Pose& pose, const VectorXd& q0,
6365
const Pose& tcp_offset = Pose::Identity()) override;
66+
Pose forward(const VectorXd& q0, const Pose& tcp_offset) override;
6467
};
6568
} // namespace common
6669
} // namespace rcs

‎src/pybind/rcs.cpp‎

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -239,10 +239,6 @@ PYBIND11_MODULE(_core, m) {
239239
.def(py::init<const std::string &, const std::string &>(),
240240
py::arg("urdf_path"), py::arg("frame_id") = "fr3_link8");
241241

242-
py::class_<rcs::common::RConfig>(common, "RConfig");
243-
py::class_<rcs::common::RState>(common, "RState");
244-
py::class_<rcs::common::GConfig>(common, "GConfig");
245-
py::class_<rcs::common::GState>(common, "GState");
246242
py::enum_<rcs::common::RobotType>(common, "RobotType")
247243
.value("FR3", rcs::common::RobotType::FR3)
248244
.value("UR5e", rcs::common::RobotType::UR5e)

‎src/sim/test.cpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,8 @@
1515
// const std::string mjcf = MODEL_DIR "/mjcf/fr3_modular/scene.xml";
1616
// const std::string urdf = MODEL_DIR "/fr3/urdf/fr3_from_panda.urdf";
1717
const std::string mjcf = "build/_deps/scenes-src/scenes/lab/scene.xml";
18-
const std::string urdf_path = "build/_deps/scenes-src/scenes/lab/assets/fr3.urdf";
18+
const std::string urdf_path =
19+
"build/_deps/scenes-src/scenes/lab/assets/fr3.urdf";
1920
static const Eigen::Matrix<double, 1, 3, Eigen::RowMajor> iso_cube_center(
2021
0.498, 0.0, 0.226);
2122
static const float iso_cube_size = 0.4;

0 commit comments

Comments
 (0)