Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion examples/fr3/fr3_direct_control.py
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@ def main():
robot_cfg = hw.FR3Config()
robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
robot_cfg.ik_solver = hw.IKSolver.rcs_ik
robot.set_parameters(robot_cfg) # type: ignore
robot.set_config(robot_cfg) # type: ignore

gripper_cfg_hw = hw.FHConfig()
gripper_cfg_hw.epsilon_inner = gripper_cfg_hw.epsilon_outer = 0.1
Expand Down
2 changes: 1 addition & 1 deletion extensions/rcs_fr3/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ with FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False):
robot_cfg = FR3Config()
robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
robot_cfg.ik_solver = IKSolver.rcs_ik
robot.set_parameters(robot_cfg)
robot.set_config(robot_cfg)

gripper_cfg_hw = hw.FHConfig()
gripper_cfg_hw.epsilon_inner = gripper_cfg_hw.epsilon_outer = 0.1
Expand Down
4 changes: 2 additions & 2 deletions extensions/rcs_fr3/src/hw/FR3.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ FR3::~FR3() {}
* @param cfg The configuration for the robot, it should be a FR3Config type
* otherwise the call will fail
*/
bool FR3::set_parameters(const FR3Config &cfg) {
bool FR3::set_config(const FR3Config &cfg) {
this->cfg = cfg;
this->cfg.speed_factor = std::min(std::max(cfg.speed_factor, 0.0), 1.0);

Expand All @@ -59,7 +59,7 @@ bool FR3::set_parameters(const FR3Config &cfg) {
return true;
}

FR3Config *FR3::get_parameters() {
FR3Config *FR3::get_config() {
// copy config to heap
FR3Config *cfg = new FR3Config();
*cfg = this->cfg;
Expand Down
4 changes: 2 additions & 2 deletions extensions/rcs_fr3/src/hw/FR3.h
Original file line number Diff line number Diff line change
Expand Up @@ -66,9 +66,9 @@ class FR3 : public common::Robot {
const std::optional<FR3Config> &cfg = std::nullopt);
~FR3() override;

bool set_parameters(const FR3Config &cfg);
bool set_config(const FR3Config &cfg);

FR3Config *get_parameters() override;
FR3Config *get_config() override;

FR3State *get_state() override;

Expand Down
4 changes: 2 additions & 2 deletions extensions/rcs_fr3/src/hw/FrankaHand.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ FrankaHand::FrankaHand(const std::string &ip, const FHConfig &cfg)

FrankaHand::~FrankaHand() {}

bool FrankaHand::set_parameters(const FHConfig &cfg) {
bool FrankaHand::set_config(const FHConfig &cfg) {
franka::GripperState gripper_state = this->gripper.readOnce();
if (gripper_state.max_width < cfg.grasping_width) {
return false;
Expand All @@ -29,7 +29,7 @@ bool FrankaHand::set_parameters(const FHConfig &cfg) {
return true;
}

FHConfig *FrankaHand::get_parameters() {
FHConfig *FrankaHand::get_config() {
// copy config to heap
FHConfig *cfg = new FHConfig();
*cfg = this->cfg;
Expand Down
4 changes: 2 additions & 2 deletions extensions/rcs_fr3/src/hw/FrankaHand.h
Original file line number Diff line number Diff line change
Expand Up @@ -57,9 +57,9 @@ class FrankaHand : public common::Gripper {
FrankaHand(const std::string &ip, const FHConfig &cfg);
~FrankaHand() override;

bool set_parameters(const FHConfig &cfg);
bool set_config(const FHConfig &cfg);

FHConfig *get_parameters() override;
FHConfig *get_config() override;

FHState *get_state() override;

Expand Down
9 changes: 4 additions & 5 deletions extensions/rcs_fr3/src/pybind/rcs.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -101,8 +101,8 @@ PYBIND11_MODULE(_core, m) {
.def(py::init<const std::string &,
std::optional<std::shared_ptr<rcs::common::IK>>>(),
py::arg("ip"), py::arg("ik") = std::nullopt)
.def("set_parameters", &rcs::hw::FR3::set_parameters, py::arg("cfg"))
.def("get_parameters", &rcs::hw::FR3::get_parameters)
.def("set_config", &rcs::hw::FR3::set_config, py::arg("cfg"))
.def("get_config", &rcs::hw::FR3::get_config)
.def("get_state", &rcs::hw::FR3::get_state)
.def("set_default_robot_behavior",
&rcs::hw::FR3::set_default_robot_behavior)
Expand Down Expand Up @@ -132,10 +132,9 @@ PYBIND11_MODULE(_core, m) {
hw, "FrankaHand", gripper)
.def(py::init<const std::string &, const rcs::hw::FHConfig &>(),
py::arg("ip"), py::arg("cfg"))
.def("get_parameters", &rcs::hw::FrankaHand::get_parameters)
.def("get_config", &rcs::hw::FrankaHand::get_config)
.def("get_state", &rcs::hw::FrankaHand::get_state)
.def("set_parameters", &rcs::hw::FrankaHand::set_parameters,
py::arg("cfg"))
.def("set_config", &rcs::hw::FrankaHand::set_config, py::arg("cfg"))
.def("is_grasped", &rcs::hw::FrankaHand::is_grasped)
.def("homing", &rcs::hw::FrankaHand::homing);

Expand Down
8 changes: 4 additions & 4 deletions extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi
Original file line number Diff line number Diff line change
Expand Up @@ -58,13 +58,14 @@ class FR3(rcs._core.common.Robot):
self, desired_q: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]]
) -> None: ...
def double_tap_robot_to_continue(self) -> None: ...
def get_parameters(self) -> FR3Config: ...
def get_config(self) -> FR3Config: ...
def get_state(self) -> FR3State: ...
def osc_set_cartesian_position(self, desired_pos_EE_in_base_frame: rcs._core.common.Pose) -> None: ...
def set_cartesian_position_ik(
self, pose: rcs._core.common.Pose, max_time: float, elbow: float | None, max_force: float | None = 5
) -> None: ...
def set_cartesian_position_internal(self, pose: rcs._core.common.Pose) -> None: ...
def set_config(self, cfg: FR3Config) -> bool: ...
def set_default_robot_behavior(self) -> None: ...
def set_guiding_mode(
self,
Expand All @@ -76,7 +77,6 @@ class FR3(rcs._core.common.Robot):
yaw: bool = True,
elbow: bool = True,
) -> None: ...
def set_parameters(self, cfg: FR3Config) -> bool: ...
def stop_control_thread(self) -> None: ...
def zero_torque_guiding(self) -> None: ...

Expand All @@ -100,11 +100,11 @@ class FR3State(rcs._core.common.RobotState):

class FrankaHand(rcs._core.common.Gripper):
def __init__(self, ip: str, cfg: FHConfig) -> None: ...
def get_parameters(self) -> FHConfig: ...
def get_config(self) -> FHConfig: ...
def get_state(self) -> FHState: ...
def homing(self) -> bool: ...
def is_grasped(self) -> bool: ...
def set_parameters(self, cfg: FHConfig) -> bool: ...
def set_config(self, cfg: FHConfig) -> bool: ...

class IKSolver:
"""
Expand Down
4 changes: 2 additions & 2 deletions extensions/rcs_fr3/src/rcs_fr3/creators.py
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ def __call__( # type: ignore
urdf_path = rcs.scenes["fr3_empty_world"].urdf
ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None
robot = hw.FR3(ip, ik)
robot.set_parameters(robot_cfg)
robot.set_config(robot_cfg)

env: gym.Env = RobotEnv(robot, ControlMode.JOINTS if collision_guard is not None else control_mode)

Expand Down Expand Up @@ -119,7 +119,7 @@ def __call__( # type: ignore
robots: dict[str, hw.FR3] = {}
for ip in ips:
robots[ip] = hw.FR3(ip, ik)
robots[ip].set_parameters(robot_cfg)
robots[ip].set_config(robot_cfg)

envs = {}
for ip in ips:
Expand Down
4 changes: 2 additions & 2 deletions extensions/rcs_fr3/src/rcs_fr3/desk.py
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False
config = rcs_fr3.hw.FR3Config()
config.speed_factor = 0.2
config.ik_solver = rcs_fr3.hw.IKSolver.franka_ik
f.set_parameters(config)
f.set_config(config)
config_hand = rcs_fr3.hw.FHConfig()
g = rcs_fr3.hw.FrankaHand(ip, config_hand)
if shut:
Expand All @@ -55,7 +55,7 @@ def info(ip: str, username: str, password: str, include_hand: bool = False):
with Desk.fci(ip, username, password):
f = rcs_fr3.hw.FR3(ip)
config = rcs_fr3.hw.FR3Config()
f.set_parameters(config)
f.set_config(config)
print("Robot info:")
print("Current cartesian position:")
print(f.get_cartesian_position())
Expand Down
4 changes: 2 additions & 2 deletions extensions/rcs_so101/src/rcs_so101/hw.py
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ def get_joint_position(self) -> np.ndarray[tuple[typing.Literal[5]], np.dtype[np
),
)

def get_parameters(self):
def get_config(self):
a = common.RobotConfig()
a.robot_platform = common.RobotPlatform.HARDWARE
a.robot_type = common.RobotType.SO101
Expand Down Expand Up @@ -83,7 +83,7 @@ def get_normalized_width(self) -> float:
obs = self._hf_robot.get_observation()
return obs["gripper.pos"] / 100.0

# def get_parameters(self) -> GripperConfig: ...
# def get_config(self) -> GripperConfig: ...
# def get_state(self) -> GripperState: ...

def grasp(self) -> None:
Expand Down
4 changes: 2 additions & 2 deletions extensions/rcs_xarm7/src/rcs_xarm7/hw.py
Original file line number Diff line number Diff line change
Expand Up @@ -61,10 +61,10 @@ def get_joint_position(self) -> np.ndarray[tuple[typing.Literal[7]], np.dtype[np
np.array(self._xarm.get_servo_angle(is_radian=True)[1]),
)

def get_parameters(self) -> XArm7Config:
def get_config(self) -> XArm7Config:
return self._config

def set_parameters(self, robot_cfg: XArm7Config) -> None:
def set_config(self, robot_cfg: XArm7Config) -> None:
self._config = robot_cfg

def get_state(self) -> common.RobotState:
Expand Down
20 changes: 10 additions & 10 deletions include/rcs/Robot.h
Original file line number Diff line number Diff line change
Expand Up @@ -128,13 +128,13 @@ class Robot {
public:
virtual ~Robot(){};

// Also add an implementation specific set_parameters function that takes
// Also add an implementation specific set_config function that takes
// a deduced config type
// This functino can also be used for signaling e.g. error recovery and the
// This function can also be used for signaling e.g. error recovery and the
// like
// bool set_parameters(const RConfig& cfg);
// bool set_config(const RConfig& cfg);

virtual RobotConfig* get_parameters() = 0;
virtual RobotConfig* get_config() = 0;

virtual RobotState* get_state() = 0;

Expand Down Expand Up @@ -167,11 +167,11 @@ class Gripper {
public:
virtual ~Gripper(){};

// Also add an implementation specific set_parameters function that takes
// Also add an implementation specific set_config function that takes
// a deduced config type
// bool set_parameters(const GConfig& cfg);
// bool set_config(const GConfig& cfg);

virtual GripperConfig* get_parameters() = 0;
virtual GripperConfig* get_config() = 0;
virtual GripperState* get_state() = 0;

// set width of the gripper, 0 is closed, 1 is open
Expand Down Expand Up @@ -200,11 +200,11 @@ class Hand {
public:
virtual ~Hand(){};
// TODO: Add low-level control interface for the hand with internal state updates
// Also add an implementation specific set_parameters function that takes
// Also add an implementation specific set_config function that takes
// a deduced config type
// bool set_parameters(const GConfig& cfg);
// bool set_config(const GConfig& cfg);

virtual HandConfig* get_parameters() = 0;
virtual HandConfig* get_config() = 0;
virtual HandState* get_state() = 0;

// set width of the hand, 0 is closed, 1 is open
Expand Down
6 changes: 3 additions & 3 deletions python/rcs/_core/common.pyi
Original file line number Diff line number Diff line change
Expand Up @@ -93,8 +93,8 @@ class GraspType:
class Gripper:
def __init__(self) -> None: ...
def close(self) -> None: ...
def get_config(self) -> GripperConfig: ...
def get_normalized_width(self) -> float: ...
def get_parameters(self) -> GripperConfig: ...
def get_state(self) -> GripperState: ...
def grasp(self) -> None: ...
def is_grasped(self) -> bool: ...
Expand All @@ -112,8 +112,8 @@ class GripperState:
class Hand:
def __init__(self) -> None: ...
def close(self) -> None: ...
def get_config(self) -> HandConfig: ...
def get_normalized_joint_poses(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ...
def get_parameters(self) -> HandConfig: ...
def get_state(self) -> HandState: ...
def grasp(self) -> None: ...
def is_grasped(self) -> bool: ...
Expand Down Expand Up @@ -222,9 +222,9 @@ class Robot:
def close(self) -> None: ...
def get_base_pose_in_world_coordinates(self) -> Pose: ...
def get_cartesian_position(self) -> Pose: ...
def get_config(self) -> RobotConfig: ...
def get_ik(self) -> IK | None: ...
def get_joint_position(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ...
def get_parameters(self) -> RobotConfig: ...
def get_state(self) -> RobotState: ...
def move_home(self) -> None: ...
def reset(self) -> None: ...
Expand Down
12 changes: 6 additions & 6 deletions python/rcs/_core/sim.pyi
Original file line number Diff line number Diff line change
Expand Up @@ -118,9 +118,9 @@ class SimConfig:

class SimGripper(rcs._core.common.Gripper):
def __init__(self, sim: Sim, cfg: SimGripperConfig) -> None: ...
def get_parameters(self) -> SimGripperConfig: ...
def get_config(self) -> SimGripperConfig: ...
def get_state(self) -> SimGripperState: ...
def set_parameters(self, cfg: SimGripperConfig) -> bool: ...
def set_config(self, cfg: SimGripperConfig) -> bool: ...

class SimGripperConfig(rcs._core.common.GripperConfig):
actuator: str
Expand Down Expand Up @@ -153,10 +153,10 @@ class SimRobot(rcs._core.common.Robot):
def __init__(
self, sim: Sim, ik: rcs._core.common.IK, cfg: SimRobotConfig, register_convergence_callback: bool = True
) -> None: ...
def get_parameters(self) -> SimRobotConfig: ...
def get_config(self) -> SimRobotConfig: ...
def get_state(self) -> SimRobotState: ...
def set_config(self, cfg: SimRobotConfig) -> bool: ...
def set_joints_hard(self, q: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]) -> None: ...
def set_parameters(self, cfg: SimRobotConfig) -> bool: ...

class SimRobotConfig(rcs._core.common.RobotConfig):
actuators: list[str]
Expand Down Expand Up @@ -190,9 +190,9 @@ class SimRobotState(rcs._core.common.RobotState):

class SimTilburgHand(rcs._core.common.Hand):
def __init__(self, sim: Sim, cfg: SimTilburgHandConfig) -> None: ...
def get_parameters(self) -> SimTilburgHandConfig: ...
def get_config(self) -> SimTilburgHandConfig: ...
def get_state(self) -> SimTilburgHandState: ...
def set_parameters(self, cfg: SimTilburgHandConfig) -> bool: ...
def set_config(self, cfg: SimTilburgHandConfig) -> bool: ...

class SimTilburgHandConfig(rcs._core.common.HandConfig):
actuators: list[str]
Expand Down
6 changes: 3 additions & 3 deletions python/rcs/envs/base.py
Original file line number Diff line number Diff line change
Expand Up @@ -176,21 +176,21 @@ class ControlMode(Enum):

def get_dof(robot: common.Robot) -> int:
"""Returns the number of degrees of freedom of the robot."""
return common.robots_meta_config(robot.get_parameters().robot_type).dof
return common.robots_meta_config(robot.get_config().robot_type).dof


def get_joint_limits(robot: common.Robot) -> tuple[np.ndarray, np.ndarray]:
"""Returns the joint limits of the robot.

The first element is the lower limit, the second element is the upper limit.
"""
limits = common.robots_meta_config(robot.get_parameters().robot_type).joint_limits
limits = common.robots_meta_config(robot.get_config().robot_type).joint_limits
return limits[0], limits[1]


def get_home_position(robot: common.Robot) -> np.ndarray:
"""Returns the home position of the robot."""
return common.robots_meta_config(robot.get_parameters().robot_type).q_home
return common.robots_meta_config(robot.get_config().robot_type).q_home


class RobotEnv(gym.Env):
Expand Down
Loading