diff --git a/assets/fr3/mjcf/fr3_0.xml b/assets/fr3/mjcf/fr3_0.xml
new file mode 100644
index 00000000..e80f5352
--- /dev/null
+++ b/assets/fr3/mjcf/fr3_0.xml
@@ -0,0 +1,198 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/assets/fr3/mjcf/fr3_common.xml b/assets/fr3/mjcf/fr3_common.xml
index 1196d6bc..a29b9dde 100644
--- a/assets/fr3/mjcf/fr3_common.xml
+++ b/assets/fr3/mjcf/fr3_common.xml
@@ -1,49 +1,7 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/assets/fr3/mjcf/fr3_unnamed.xml b/assets/fr3/mjcf/fr3_unnamed.xml
index bc7e1718..6577053b 100644
--- a/assets/fr3/mjcf/fr3_unnamed.xml
+++ b/assets/fr3/mjcf/fr3_unnamed.xml
@@ -1,4 +1,50 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -13,33 +59,33 @@
-
+
-
+
-
+
-
+
-
+
@@ -47,7 +93,7 @@
-
+
@@ -60,7 +106,7 @@
-
+
diff --git a/assets/scenes/fr3_empty_world/fr3_0.xml b/assets/scenes/fr3_empty_world/fr3_0.xml
deleted file mode 100644
index 203e29c5..00000000
--- a/assets/scenes/fr3_empty_world/fr3_0.xml
+++ /dev/null
@@ -1,155 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/assets/scenes/fr3_empty_world/fr3_0.xml b/assets/scenes/fr3_empty_world/fr3_0.xml
new file mode 120000
index 00000000..378af610
--- /dev/null
+++ b/assets/scenes/fr3_empty_world/fr3_0.xml
@@ -0,0 +1 @@
+../../fr3/mjcf/fr3_0.xml
\ No newline at end of file
diff --git a/assets/scenes/fr3_simple_pick_up/fr3_0.xml b/assets/scenes/fr3_simple_pick_up/fr3_0.xml
deleted file mode 100644
index 3695e1b7..00000000
--- a/assets/scenes/fr3_simple_pick_up/fr3_0.xml
+++ /dev/null
@@ -1,152 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/assets/scenes/fr3_simple_pick_up/fr3_0.xml b/assets/scenes/fr3_simple_pick_up/fr3_0.xml
new file mode 120000
index 00000000..378af610
--- /dev/null
+++ b/assets/scenes/fr3_simple_pick_up/fr3_0.xml
@@ -0,0 +1 @@
+../../fr3/mjcf/fr3_0.xml
\ No newline at end of file
diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py
index be519b43..cde695ae 100644
--- a/python/rcs/envs/creators.py
+++ b/python/rcs/envs/creators.py
@@ -83,7 +83,10 @@ def __call__( # type: ignore
mjcf = rcs.scenes[mjcf]["mjb"]
simulation = sim.Sim(mjcf)
+ # ik = rcs.common.Pin("/home/juelg/code/internal_rcs/assets/scenes/fr3_empty_world/fr3_0.xml", "attachment_site_0", urdf=False)
+ # ik = rcs.common.Pin(str(urdf_path))
ik = rcs.common.RL(str(urdf_path))
+
robot = rcs.sim.SimRobot(simulation, ik, robot_cfg)
env: gym.Env = RobotEnv(robot, control_mode)
env = RobotSimWrapper(env, simulation, sim_wrapper)
diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py
index 781391a3..eea965c9 100644
--- a/python/rcs/envs/utils.py
+++ b/python/rcs/envs/utils.py
@@ -68,7 +68,6 @@ def default_mujoco_cameraset_cfg() -> dict[str, SimCameraConfig]:
}
-# TODO: franka hand offset needs to be removed here
def get_tcp_offset(mjcf: str | Path) -> rcs.common.Pose:
"""Reads out tcp offset set in mjcf file.
@@ -89,13 +88,15 @@ def get_tcp_offset(mjcf: str | Path) -> rcs.common.Pose:
elif mjcf.suffix == ".mjb":
model = mj.MjModel.from_binary_path(str(mjcf))
else:
- msg = f"Expected .mjb, .mjcf or.xml, got {mjcf.suffix}"
+ msg = f"Expected .mjb, .mjcf or.xml, got {mjcf.suffix} and {mjcf}"
raise AssertionError(msg)
try:
- tcp_offset = np.array(model.numeric("tcp_offset").data)
- pose_offset = rcs.common.Pose(translation=tcp_offset)
- return rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) * pose_offset
+ tcp_offset_translation = np.array(model.numeric("tcp_offset_translation").data)
+ tcp_offset_rotation_matrix = np.array(model.numeric("tcp_offset_rotation_matrix").data)
+ return rcs.common.Pose(
+ translation=tcp_offset_translation, rotation=tcp_offset_rotation_matrix.reshape((3, 3))
+ ) # type: ignore
except KeyError:
msg = "No tcp offset found in the model. Using the default tcp offset."
logging.info(msg)
- return rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
+ return rcs.common.Pose()
diff --git a/src/rcs/IK.cpp b/src/rcs/IK.cpp
index 98f466ab..1643c4db 100644
--- a/src/rcs/IK.cpp
+++ b/src/rcs/IK.cpp
@@ -78,7 +78,9 @@ Pin::Pin(const std::string& path, const std::string& frame_id, bool urdf = true)
std::optional Pin::ik(const Pose& pose, const VectorXd& q0,
const Pose& tcp_offset) {
rcs::common::Pose new_pose = pose * tcp_offset.inverse();
- VectorXd q(q0);
+ VectorXd q(model.nq);
+ q.setZero();
+ q.head(q0.size()) = q0;
const pinocchio::SE3 oMdes(new_pose.rotation_m(), new_pose.translation());
pinocchio::Data::Matrix6x J(6, model.nv);
J.setZero();
@@ -117,8 +119,10 @@ std::optional Pin::ik(const Pose& pose, const VectorXd& q0,
Pose Pin::forward(const VectorXd& q0, const Pose& tcp_offset) {
// pose is assumed to be in the robots coordinate frame
- pinocchio::forwardKinematics(model, data, q0);
- pinocchio::updateFramePlacements(model, data);
+ VectorXd q(model.nq);
+ q.setZero();
+ q.head(q0.size()) = q0;
+ pinocchio::framesForwardKinematics(model, data, q);
rcs::common::Pose pose(data.oMf[this->FRAME_ID].rotation(),
data.oMf[this->FRAME_ID].translation());