diff --git a/Makefile b/Makefile index bbb7bf92..6bbef537 100644 --- a/Makefile +++ b/Makefile @@ -30,9 +30,11 @@ stubgen: find ./python -not -path "./python/rcs/_core/*" -name '*.pyi' -delete find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/tuple\[typing\.Literal[\1]\]/g' find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/tuple\[\1\]/g' - ruff check --fix python/rcs/_core - isort python/rcs/_core - black python/rcs/_core + sed -i 's/ q_home: numpy\.ndarray\[tuple\[M\], numpy\.dtype\[numpy\.float64\]\] | None/ q_home: numpy.ndarray | None/' python/rcs/_core/common.pyi + python scripts/generate_common_typing.py + ruff check --fix python/rcs/_core python/rcs/common_typing.py + isort python/rcs/_core python/rcs/common_typing.py + black python/rcs/_core python/rcs/common_typing.py # Python pycheckformat: diff --git a/examples/fr3/fr3_direct_control.py b/examples/fr3/fr3_direct_control.py index dd485575..7d8ea15e 100644 --- a/examples/fr3/fr3_direct_control.py +++ b/examples/fr3/fr3_direct_control.py @@ -97,21 +97,20 @@ def main(): simulation.open_gui() else: - fr3_cfg = default_fr3_hw_robot_cfg() + fr3_cfg = default_fr3_hw_robot_cfg(ROBOT_IP) fr3_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) ik = rcs.common.Pin( fr3_cfg.kinematic_model_path, fr3_cfg.attachment_site, urdf=fr3_cfg.kinematic_model_path.endswith(".urdf"), ) - robot = hw.Franka(ROBOT_IP, ik) - robot.set_config(fr3_cfg) # type: ignore + robot = hw.Franka(fr3_cfg, ik) - gripper_cfg_hw = hw.FHConfig() + gripper_cfg_hw = hw.FHConfig(ip=ROBOT_IP) gripper_cfg_hw.epsilon_inner = gripper_cfg_hw.epsilon_outer = 0.1 gripper_cfg_hw.speed = 0.1 gripper_cfg_hw.force = 30 - gripper = hw.FrankaHand(ROBOT_IP, gripper_cfg_hw) + gripper = hw.FrankaHand(gripper_cfg_hw) input("the robot is going to move, press enter whenever you are ready") # move to home position and open gripper diff --git a/examples/fr3/fr3_readme.py b/examples/fr3/fr3_readme.py index 6e2dd5f3..2e4af208 100644 --- a/examples/fr3/fr3_readme.py +++ b/examples/fr3/fr3_readme.py @@ -48,7 +48,7 @@ # gripper gripper = sim.SimGripper(simulation, gripper_cfg) - env = GripperWrapper(env, gripper, binary=True) + env = GripperWrapper(env, gripper) env = RobotSimWrapper(env) env = GripperWrapperSim(env) diff --git a/examples/ur5e/ur5e_env_cartesian_control.py b/examples/ur5e/ur5e_env_cartesian_control.py index 97cc9edb..824ac522 100644 --- a/examples/ur5e/ur5e_env_cartesian_control.py +++ b/examples/ur5e/ur5e_env_cartesian_control.py @@ -20,12 +20,11 @@ def main(): if ROBOT_INSTANCE == RobotPlatform.HARDWARE: - robot_cfg = UR5eConfig() + robot_cfg = UR5eConfig(ip=ROBOT_IP) robot_cfg.async_control = False env_rel = RCSUR5eEnvCreator()( robot_cfg=robot_cfg, control_mode=ControlMode.CARTESIAN_TQuat, - ip=ROBOT_IP, camera_set=None, max_relative_movement=0.2, relative_to=RelativeTo.LAST_STEP, diff --git a/examples/ur5e/ur5e_env_joint_control.py b/examples/ur5e/ur5e_env_joint_control.py index 7caee7e2..679827e8 100644 --- a/examples/ur5e/ur5e_env_joint_control.py +++ b/examples/ur5e/ur5e_env_joint_control.py @@ -22,11 +22,10 @@ def main(): if ROBOT_INSTANCE == RobotPlatform.HARDWARE: - robot_cfg = UR5eConfig() + robot_cfg = UR5eConfig(ip=ROBOT_IP) env_rel = RCSUR5eEnvCreator()( control_mode=ControlMode.JOINTS, robot_cfg=robot_cfg, - ip=ROBOT_IP, camera_set=None, max_relative_movement=np.deg2rad(5), relative_to=RelativeTo.LAST_STEP, diff --git a/examples/xarm7/xarm7_env_cartesian_control.py b/examples/xarm7/xarm7_env_cartesian_control.py index 91cce45a..46e44412 100644 --- a/examples/xarm7/xarm7_env_cartesian_control.py +++ b/examples/xarm7/xarm7_env_cartesian_control.py @@ -5,6 +5,7 @@ from rcs.envs.base import ControlMode, RelativeTo from rcs.envs.creators import SimEnvCreator from rcs_xarm7.creators import RCSXArm7EnvCreator +from rcs_xarm7.hw import XArm7Config import rcs from rcs import sim @@ -30,15 +31,16 @@ def main(): if ROBOT_INSTANCE == RobotPlatform.HARDWARE: + robot_cfg = XArm7Config(ip=ROBOT_IP) env_rel = RCSXArm7EnvCreator()( + robot_cfg=robot_cfg, control_mode=ControlMode.CARTESIAN_TQuat, - ip=ROBOT_IP, relative_to=RelativeTo.LAST_STEP, max_relative_movement=0.5, ) else: - robot_cfg = sim.SimRobotConfig() - robot_cfg.actuators = [ + robot_sim_cfg = sim.SimRobotConfig() + robot_sim_cfg.actuators = [ "act1", "act2", "act3", @@ -47,7 +49,7 @@ def main(): "act6", "act7", ] - robot_cfg.joints = [ + robot_sim_cfg.joints = [ "joint1", "joint2", "joint3", @@ -56,15 +58,15 @@ def main(): "joint6", "joint7", ] - robot_cfg.base = "base" - robot_cfg.robot_type = rcs.common.RobotType.XArm7 - robot_cfg.attachment_site = "attachment_site" - robot_cfg.arm_collision_geoms = [] + robot_sim_cfg.base = "base" + robot_sim_cfg.robot_type = rcs.common.RobotType.XArm7 + robot_sim_cfg.attachment_site = "attachment_site" + robot_sim_cfg.arm_collision_geoms = [] scene = rcs.scenes["xarm7_empty_world"] - robot_cfg.mjcf_scene_path = scene.mjb or scene.mjcf_scene - robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot + robot_sim_cfg.mjcf_scene_path = scene.mjb or scene.mjcf_scene + robot_sim_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot env_rel = SimEnvCreator()( - robot_cfg=robot_cfg, + robot_cfg=robot_sim_cfg, control_mode=ControlMode.CARTESIAN_TQuat, gripper_cfg=None, # cameras=default_mujoco_cameraset_cfg(), diff --git a/examples/xarm7/xarm7_env_joint_control.py b/examples/xarm7/xarm7_env_joint_control.py index 9cfcc17b..f648e6a1 100644 --- a/examples/xarm7/xarm7_env_joint_control.py +++ b/examples/xarm7/xarm7_env_joint_control.py @@ -6,6 +6,7 @@ from rcs.envs.base import ControlMode, RelativeTo from rcs.envs.creators import SimEnvCreator from rcs_xarm7.creators import RCSXArm7EnvCreator +from rcs_xarm7.hw import XArm7Config import rcs from rcs import sim @@ -30,15 +31,16 @@ def main(): if ROBOT_INSTANCE == RobotPlatform.HARDWARE: + robot_cfg = XArm7Config(ip=ROBOT_IP) env_rel = RCSXArm7EnvCreator()( + robot_cfg=robot_cfg, control_mode=ControlMode.JOINTS, - ip=ROBOT_IP, relative_to=RelativeTo.LAST_STEP, max_relative_movement=np.deg2rad(5), ) else: - robot_cfg = sim.SimRobotConfig() - robot_cfg.actuators = [ + robot_sim_cfg = sim.SimRobotConfig() + robot_sim_cfg.actuators = [ "act1", "act2", "act3", @@ -47,7 +49,7 @@ def main(): "act6", "act7", ] - robot_cfg.joints = [ + robot_sim_cfg.joints = [ "joint1", "joint2", "joint3", @@ -56,15 +58,15 @@ def main(): "joint6", "joint7", ] - robot_cfg.base = "base" - robot_cfg.robot_type = rcs.common.RobotType.XArm7 - robot_cfg.attachment_site = "attachment_site" - robot_cfg.arm_collision_geoms = [] + robot_sim_cfg.base = "base" + robot_sim_cfg.robot_type = rcs.common.RobotType.XArm7 + robot_sim_cfg.attachment_site = "attachment_site" + robot_sim_cfg.arm_collision_geoms = [] scene = rcs.scenes["xarm7_empty_world"] - robot_cfg.mjcf_scene_path = scene.mjb or scene.mjcf_scene - robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot + robot_sim_cfg.mjcf_scene_path = scene.mjb or scene.mjcf_scene + robot_sim_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot env_rel = SimEnvCreator()( - robot_cfg=robot_cfg, + robot_cfg=robot_sim_cfg, control_mode=ControlMode.JOINTS, gripper_cfg=None, # cameras=default_mujoco_cameraset_cfg(), diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index d2b0b9e3..a56dedaf 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -19,17 +19,15 @@ namespace rcs { namespace hw { -Franka::Franka(const std::string& ip, - std::optional> ik, - const std::optional& cfg) - : robot(ip), m_ik(ik) { +Franka::Franka(const FrankaConfig& cfg, + std::optional> ik) + : m_cfg(cfg), + m_ik(ik), + robot(cfg.ip, cfg.ignore_realtime ? franka::RealtimeConfig::kIgnore + : franka::RealtimeConfig::kEnforce) { // set collision behavior and impedance this->set_default_robot_behavior(); this->set_guiding_mode(true, true, true, true, true, true, true); - - if (cfg.has_value()) { - this->cfg = cfg.value(); - } // else default constructor } Franka::~Franka() { @@ -46,11 +44,11 @@ Franka::~Franka() { * otherwise the call will fail */ bool Franka::set_config(const FrankaConfig& cfg) { - this->cfg = cfg; - this->cfg.speed_factor = std::min(std::max(cfg.speed_factor, 0.0), 1.0); + this->m_cfg = cfg; + this->m_cfg.speed_factor = std::min(std::max(cfg.speed_factor, 0.0), 1.0); - if (this->cfg.load_parameters.has_value()) { - auto load_value = &(this->cfg.load_parameters.value()); + if (this->m_cfg.load_parameters.has_value()) { + auto load_value = &(this->m_cfg.load_parameters.value()); if (!load_value->f_x_cload.has_value()) { load_value->f_x_cload = Eigen::Vector3d::Zero(); } @@ -69,7 +67,7 @@ bool Franka::set_config(const FrankaConfig& cfg) { FrankaConfig* Franka::get_config() { // copy config to heap FrankaConfig* cfg = new FrankaConfig(); - *cfg = this->cfg; + *cfg = this->m_cfg; return cfg; } @@ -110,19 +108,19 @@ common::Pose Franka::get_cartesian_position() { x = common::Pose(this->curr_state.O_T_EE); this->interpolator_mutex.unlock(); } - if (!this->cfg.tcp_offset_configured_in_desk) { - return x * cfg.tcp_offset; + if (!this->m_cfg.tcp_offset_configured_in_desk) { + return x * this->m_cfg.tcp_offset; } return x; } void Franka::set_joint_position(const common::VectorXd& q) { - if (this->cfg.async_control) { + if (this->m_cfg.async_control) { this->controller_set_joint_position(q); return; } // sync control - FrankaMotionGenerator motion_generator(this->cfg.speed_factor, q); + FrankaMotionGenerator motion_generator(this->m_cfg.speed_factor, q); this->robot.control(motion_generator); } @@ -635,8 +633,8 @@ void Franka::move_home() { // sync this->stop_control_thread(); FrankaMotionGenerator motion_generator( - this->cfg.speed_factor, - common::robots_meta_config.at(this->cfg.robot_type).q_home); + this->m_cfg.speed_factor, + common::robots_meta_config.at(this->m_cfg.robot_type).q_home); this->robot.control(motion_generator); } @@ -713,28 +711,28 @@ std::optional> Franka::get_ik() { void Franka::set_cartesian_position(const common::Pose& x) { // pose is assumed to be in the robots coordinate frame - if (this->cfg.async_control) { + if (this->m_cfg.async_control) { this->osc_set_cartesian_position(x); return; } // TODO: this should handled with tcp offset config common::Pose nominal_end_effector_frame_value; - if (this->cfg.nominal_end_effector_frame.has_value()) { + if (this->m_cfg.nominal_end_effector_frame.has_value()) { nominal_end_effector_frame_value = - this->cfg.nominal_end_effector_frame.value(); + this->m_cfg.nominal_end_effector_frame.value(); } else { nominal_end_effector_frame_value = common::Pose::Identity(); } // nominal end effector frame should be on top of tcp offset as franka already // takes care of the default franka hand offset lets add a franka hand offset - if (this->cfg.ik_solver == IKSolver::franka_ik) { + if (this->m_cfg.ik_solver == IKSolver::franka_ik) { // if gripper is attached the tcp offset will automatically be applied // by libfranka this->robot.setEE(nominal_end_effector_frame_value.affine_array()); this->set_cartesian_position_internal(x, 1.0, std::nullopt, std::nullopt); - } else if (this->cfg.ik_solver == IKSolver::rcs_ik) { + } else if (this->m_cfg.ik_solver == IKSolver::rcs_ik) { this->set_cartesian_position_ik(x); } } @@ -746,7 +744,7 @@ void Franka::set_cartesian_position_ik(const common::Pose& pose) { "position."); } auto joints = this->m_ik.value()->inverse(pose, this->get_joint_position(), - this->cfg.tcp_offset); + this->m_cfg.tcp_offset); if (joints.has_value()) { this->set_joint_position(joints.value()); @@ -758,8 +756,9 @@ void Franka::set_cartesian_position_ik(const common::Pose& pose) { } common::Pose Franka::get_base_pose_in_world_coordinates() { - return this->cfg.world_to_robot.has_value() ? this->cfg.world_to_robot.value() - : common::Pose(); + return this->m_cfg.world_to_robot.has_value() + ? this->m_cfg.world_to_robot.value() + : common::Pose(); } void Franka::set_cartesian_position_internal(const common::Pose& pose, diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h index 789d18a4..662cf850 100644 --- a/extensions/rcs_fr3/src/hw/Franka.h +++ b/extensions/rcs_fr3/src/hw/Franka.h @@ -32,9 +32,7 @@ enum IKSolver { franka_ik = 0, rcs_ik }; // control enum Controller { none = 0, jsc, osc, ztc }; struct FrankaConfig : common::RobotConfig { - // TODO: max force and elbow? - // TODO: we can either write specific bindings for each, or we use python - // dictionaries with these objects + std::string ip; common::RobotType robot_type = common::RobotType::FR3; common::RobotPlatform robot_platform = common::RobotPlatform::HARDWARE; IKSolver ik_solver = IKSolver::rcs_ik; @@ -44,6 +42,7 @@ struct FrankaConfig : common::RobotConfig { std::optional world_to_robot = std::nullopt; bool async_control = false; bool tcp_offset_configured_in_desk = true; + bool ignore_realtime = false; }; struct FR3Config : FrankaConfig {}; @@ -58,7 +57,7 @@ struct FrankaState : common::RobotState { class Franka : public common::Robot { private: franka::Robot robot; - FrankaConfig cfg; + FrankaConfig m_cfg; std::optional> m_ik; std::optional control_thread = std::nullopt; common::LinearPoseTrajInterpolator traj_interpolator; @@ -75,9 +74,8 @@ class Franka : public common::Robot { void check_for_background_errors(); public: - Franka(const std::string& ip, - std::optional> ik = std::nullopt, - const std::optional& cfg = std::nullopt); + Franka(const FrankaConfig& cfg, + std::optional> ik = std::nullopt); ~Franka() override; bool set_config(const FrankaConfig& cfg); diff --git a/extensions/rcs_fr3/src/hw/FrankaHand.cpp b/extensions/rcs_fr3/src/hw/FrankaHand.cpp index 8e6b9005..b7828873 100644 --- a/extensions/rcs_fr3/src/hw/FrankaHand.cpp +++ b/extensions/rcs_fr3/src/hw/FrankaHand.cpp @@ -12,9 +12,7 @@ namespace rcs { namespace hw { -FrankaHand::FrankaHand(const std::string& ip, const FHConfig& cfg) - : gripper(ip), cfg{} { - this->cfg = cfg; +FrankaHand::FrankaHand(const FHConfig& cfg) : gripper(cfg.ip), m_cfg(cfg) { this->m_reset(); } @@ -31,20 +29,20 @@ bool FrankaHand::set_config(const FHConfig& cfg) { if (gripper_state.max_width < cfg.grasping_width) { return false; } - this->cfg = cfg; + this->m_cfg = cfg; return true; } FHConfig* FrankaHand::get_config() { // copy config to heap FHConfig* cfg = new FHConfig(); - *cfg = this->cfg; + *cfg = this->m_cfg; return cfg; } FHState* FrankaHand::get_state() { franka::GripperState gripper_state = this->gripper.readOnce(); - if (std::abs(gripper_state.max_width - this->cfg.grasping_width) > 0.01) { + if (std::abs(gripper_state.max_width - this->m_cfg.grasping_width) > 0.01) { this->max_width = gripper_state.max_width - 0.001; } FHState* state = new FHState(); @@ -67,10 +65,10 @@ void FrankaHand::set_normalized_width(double width, double force) { width = width * gripper_state.max_width; this->last_commanded_width = width; if (force < 0.01) { - this->gripper.move(width, this->cfg.speed); + this->gripper.move(width, this->m_cfg.speed); } else { - this->gripper.grasp(width, this->cfg.speed, force, this->cfg.epsilon_inner, - this->cfg.epsilon_outer); + this->gripper.grasp(width, this->m_cfg.speed, force, + this->m_cfg.epsilon_inner, this->m_cfg.epsilon_outer); } } double FrankaHand::get_normalized_width() { @@ -102,7 +100,7 @@ void FrankaHand::m_reset() { this->max_width = gripper_state.max_width - 0.001; this->last_commanded_width = this->max_width; this->is_moving = true; - this->gripper.move(this->max_width, this->cfg.speed); + this->gripper.move(this->max_width, this->m_cfg.speed); this->is_moving = false; } void FrankaHand::reset() { this->m_reset(); } @@ -126,9 +124,9 @@ void FrankaHand::grasp() { return; } this->last_commanded_width = 0; - if (!this->cfg.async_control) { + if (!this->m_cfg.async_control) { this->is_moving = true; - this->gripper.grasp(0, this->cfg.speed, this->cfg.force, 1, 1); + this->gripper.grasp(0, this->m_cfg.speed, this->m_cfg.force, 1, 1); this->is_moving = false; return; } @@ -136,7 +134,7 @@ void FrankaHand::grasp() { this->control_thread = std::thread([&]() { this->is_moving = true; try { - this->gripper.grasp(0, this->cfg.speed, this->cfg.force, 1, 1); + this->gripper.grasp(0, this->m_cfg.speed, this->m_cfg.force, 1, 1); } catch (const franka::CommandException& e) { std::cerr << "franka hand command exception ignored grasp" << std::endl; } @@ -149,9 +147,9 @@ void FrankaHand::open() { return; } this->last_commanded_width = this->max_width; - if (!this->cfg.async_control) { + if (!this->m_cfg.async_control) { this->is_moving = true; - this->gripper.move(this->max_width, this->cfg.speed); + this->gripper.move(this->max_width, this->m_cfg.speed); this->is_moving = false; return; } @@ -160,9 +158,9 @@ void FrankaHand::open() { this->is_moving = true; try { // perhaps we should use graps here - this->gripper.move(this->max_width, this->cfg.speed); - // this->gripper.grasp(this->max_width, this->cfg.speed, this->cfg.force, - // 1, 1); + this->gripper.move(this->max_width, this->m_cfg.speed); + // this->gripper.grasp(this->max_width, this->m_cfg.speed, + // this->m_cfg.force, 1, 1); } catch (const franka::CommandException& e) { std::cerr << "franka hand command exception ignored open" << std::endl; } @@ -174,16 +172,16 @@ void FrankaHand::shut() { return; } this->last_commanded_width = 0; - if (!this->cfg.async_control) { + if (!this->m_cfg.async_control) { this->is_moving = true; - this->gripper.move(0, this->cfg.speed); + this->gripper.move(0, this->m_cfg.speed); this->is_moving = false; return; } this->m_wait(); this->control_thread = std::thread([&]() { this->is_moving = true; - this->gripper.move(0, this->cfg.speed); + this->gripper.move(0, this->m_cfg.speed); this->is_moving = false; }); } diff --git a/extensions/rcs_fr3/src/hw/FrankaHand.h b/extensions/rcs_fr3/src/hw/FrankaHand.h index 58a723da..592cc48e 100644 --- a/extensions/rcs_fr3/src/hw/FrankaHand.h +++ b/extensions/rcs_fr3/src/hw/FrankaHand.h @@ -21,6 +21,7 @@ namespace rcs { namespace hw { struct FHConfig : common::GripperConfig { + std::string ip; double grasping_width = 0.05; double speed = 0.1; double force = 5; @@ -43,7 +44,7 @@ struct FHState : common::GripperState { class FrankaHand : public common::Gripper { private: franka::Gripper gripper; - FHConfig cfg; + FHConfig m_cfg; double max_width; double last_commanded_width; // TODO: might be better if is_moving is a lock @@ -54,7 +55,7 @@ class FrankaHand : public common::Gripper { void m_wait(); public: - FrankaHand(const std::string& ip, const FHConfig& cfg); + FrankaHand(const FHConfig& cfg); ~FrankaHand() override; bool set_config(const FHConfig& cfg); diff --git a/extensions/rcs_fr3/src/hw/main.cpp b/extensions/rcs_fr3/src/hw/main.cpp index 4873e726..b971f5d8 100644 --- a/extensions/rcs_fr3/src/hw/main.cpp +++ b/extensions/rcs_fr3/src/hw/main.cpp @@ -19,7 +19,9 @@ int main() { try { auto ik = make_shared(mjcf_path, "attachment_site_0", false); - rcs::hw::Franka robot(ip, ik); + rcs::hw::FrankaConfig cfg; + cfg.ip = ip; + rcs::hw::Franka robot(cfg, ik); robot.automatic_error_recovery(); std::cout << "WARNING: This example will move the robot! " << "Please make sure to have the user stop button at hand!" diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp index 54b8171a..768bfa95 100644 --- a/extensions/rcs_fr3/src/pybind/rcs.cpp +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -126,7 +126,6 @@ PYBIND11_MODULE(_core, m) { py::object robot_config = (py::object)py::module_::import("rcs").attr("common").attr("RobotConfig"); py::class_(hw, "FrankaConfig", robot_config) - .def(py::init<>()) .def_readwrite("ik_solver", &rcs::hw::FrankaConfig::ik_solver) .def_readwrite("speed_factor", &rcs::hw::FrankaConfig::speed_factor) .def_readwrite("load_parameters", &rcs::hw::FrankaConfig::load_parameters) @@ -135,18 +134,121 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("world_to_robot", &rcs::hw::FrankaConfig::world_to_robot) .def_readwrite("tcp_offset_configured_in_desk", &rcs::hw::FrankaConfig::tcp_offset_configured_in_desk) - .def_readwrite("async_control", &rcs::hw::FrankaConfig::async_control); + .def_readwrite("async_control", &rcs::hw::FrankaConfig::async_control) + .def_readwrite("ignore_realtime", &rcs::hw::FrankaConfig::ignore_realtime) + .def_readwrite("ip", &rcs::hw::FrankaConfig::ip); + rcs::hw::FR3Config default_fr3_config; py::class_(hw, "FR3Config") - .def(py::init<>()); + .def(py::init( + [](const std::string& ip, rcs::hw::IKSolver ik_solver, + double speed_factor, + std::optional load_parameters, + std::optional nominal_end_effector_frame, + std::optional world_to_robot, + bool async_control, bool tcp_offset_configured_in_desk, + bool ignore_realtime, rcs::common::Pose tcp_offset, + std::string attachment_site, + std::string kinematic_model_path) { + rcs::hw::FR3Config cfg; + cfg.ik_solver = ik_solver; + cfg.speed_factor = speed_factor; + cfg.load_parameters = load_parameters; + cfg.nominal_end_effector_frame = nominal_end_effector_frame; + cfg.world_to_robot = world_to_robot; + cfg.async_control = async_control; + cfg.tcp_offset_configured_in_desk = + tcp_offset_configured_in_desk; + cfg.ignore_realtime = ignore_realtime; + cfg.ip = ip; + cfg.tcp_offset = tcp_offset; + cfg.attachment_site = attachment_site; + cfg.kinematic_model_path = kinematic_model_path; + return cfg; + }), + py::arg("ip"), py::arg("ik_solver") = default_fr3_config.ik_solver, + py::arg("speed_factor") = default_fr3_config.speed_factor, + py::arg("load_parameters") = default_fr3_config.load_parameters, + py::arg("nominal_end_effector_frame") = + default_fr3_config.nominal_end_effector_frame, + py::arg("world_to_robot") = default_fr3_config.world_to_robot, + py::arg("async_control") = default_fr3_config.async_control, + py::arg("tcp_offset_configured_in_desk") = + default_fr3_config.tcp_offset_configured_in_desk, + py::arg("ignore_realtime") = default_fr3_config.ignore_realtime, + py::arg("tcp_offset") = default_fr3_config.tcp_offset, + py::arg("attachment_site") = default_fr3_config.attachment_site, + py::arg("kinematic_model_path") = + default_fr3_config.kinematic_model_path); + rcs::hw::PandaConfig default_panda_config; py::class_(hw, "PandaConfig") - .def(py::init<>()); + .def(py::init( + [](const std::string& ip, rcs::hw::IKSolver ik_solver, + double speed_factor, + std::optional load_parameters, + std::optional nominal_end_effector_frame, + std::optional world_to_robot, + bool async_control, bool tcp_offset_configured_in_desk, + bool ignore_realtime, rcs::common::Pose tcp_offset, + std::string attachment_site, + std::string kinematic_model_path) { + rcs::hw::PandaConfig cfg; + cfg.ik_solver = ik_solver; + cfg.speed_factor = speed_factor; + cfg.load_parameters = load_parameters; + cfg.nominal_end_effector_frame = nominal_end_effector_frame; + cfg.world_to_robot = world_to_robot; + cfg.async_control = async_control; + cfg.tcp_offset_configured_in_desk = + tcp_offset_configured_in_desk; + cfg.ignore_realtime = ignore_realtime; + cfg.ip = ip; + cfg.tcp_offset = tcp_offset; + cfg.attachment_site = attachment_site; + cfg.kinematic_model_path = kinematic_model_path; + return cfg; + }), + py::arg("ip"), py::arg("ik_solver") = default_panda_config.ik_solver, + py::arg("speed_factor") = default_panda_config.speed_factor, + py::arg("load_parameters") = default_panda_config.load_parameters, + py::arg("nominal_end_effector_frame") = + default_panda_config.nominal_end_effector_frame, + py::arg("world_to_robot") = default_panda_config.world_to_robot, + py::arg("async_control") = default_panda_config.async_control, + py::arg("tcp_offset_configured_in_desk") = + default_panda_config.tcp_offset_configured_in_desk, + py::arg("ignore_realtime") = default_panda_config.ignore_realtime, + py::arg("tcp_offset") = default_panda_config.tcp_offset, + py::arg("attachment_site") = default_panda_config.attachment_site, + py::arg("kinematic_model_path") = + default_panda_config.kinematic_model_path); py::object gripper_config = (py::object)py::module_::import("rcs").attr("common").attr( "GripperConfig"); + rcs::hw::FHConfig default_gripper_config; py::class_(hw, "FHConfig", gripper_config) - .def(py::init<>()) + .def(py::init([](const std::string& ip, double grasping_width, + double speed, double force, double epsilon_inner, + double epsilon_outer, bool async_control) { + rcs::hw::FHConfig cfg; + cfg.ip = ip; + cfg.grasping_width = grasping_width; + cfg.speed = speed; + cfg.force = force; + cfg.epsilon_inner = epsilon_inner; + cfg.epsilon_outer = epsilon_outer; + cfg.async_control = async_control; + return cfg; + }), + py::arg("ip"), + py::arg("grasping_width") = default_gripper_config.grasping_width, + py::arg("speed") = default_gripper_config.speed, + py::arg("force") = default_gripper_config.force, + py::arg("epsilon_inner") = default_gripper_config.epsilon_inner, + py::arg("epsilon_outer") = default_gripper_config.epsilon_outer, + py::arg("async_control") = default_gripper_config.async_control) + .def_readwrite("ip", &rcs::hw::FHConfig::ip) .def_readwrite("grasping_width", &rcs::hw::FHConfig::grasping_width) .def_readwrite("speed", &rcs::hw::FHConfig::speed) .def_readwrite("force", &rcs::hw::FHConfig::force) @@ -173,11 +275,9 @@ PYBIND11_MODULE(_core, m) { (py::object)py::module_::import("rcs").attr("common").attr("Robot"); py::class_>(hw, "Franka", robot) - .def(py::init>, - const std::optional&>(), - py::arg("ip"), py::arg("ik") = std::nullopt, - py::arg("cfg") = std::nullopt) + .def(py::init>>(), + py::arg("cfg"), py::arg("ik") = std::nullopt) .def("set_config", &rcs::hw::Franka::set_config, py::arg("cfg")) .def("get_config", &rcs::hw::Franka::get_config) .def("get_state", &rcs::hw::Franka::get_state) @@ -209,8 +309,7 @@ PYBIND11_MODULE(_core, m) { (py::object)py::module_::import("rcs").attr("common").attr("Gripper"); py::class_>( hw, "FrankaHand", gripper) - .def(py::init(), - py::arg("ip"), py::arg("cfg")) + .def(py::init(), py::arg("cfg")) .def("get_config", &rcs::hw::FrankaHand::get_config) .def("get_state", &rcs::hw::FrankaHand::get_state) .def("set_config", &rcs::hw::FrankaHand::set_config, py::arg("cfg")) diff --git a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi index 3c1c9082..0af7ab19 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi +++ b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi @@ -43,8 +43,18 @@ class FHConfig(rcs._core.common.GripperConfig): epsilon_outer: float force: float grasping_width: float + ip: str speed: float - def __init__(self) -> None: ... + def __init__( + self, + ip: str, + grasping_width: float = 0.05, + speed: float = 0.1, + force: float = 5.0, + epsilon_inner: float = 0.005, + epsilon_outer: float = 0.005, + async_control: bool = False, + ) -> None: ... class FHState(rcs._core.common.GripperState): def __init__(self) -> None: ... @@ -64,9 +74,7 @@ class FHState(rcs._core.common.GripperState): def width(self) -> float: ... class Franka(rcs._core.common.Robot): - def __init__( - self, ip: str, ik: rcs._core.common.Kinematics | None = None, cfg: FrankaConfig | None = None - ) -> None: ... + def __init__(self, cfg: FrankaConfig, ik: rcs._core.common.Kinematics | None = None) -> None: ... def automatic_error_recovery(self) -> None: ... def controller_set_joint_position( self, desired_q: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] @@ -96,16 +104,17 @@ class Franka(rcs._core.common.Robot): class FrankaConfig(rcs._core.common.RobotConfig): async_control: bool + ignore_realtime: bool ik_solver: IKSolver + ip: str load_parameters: FrankaLoad | None nominal_end_effector_frame: rcs._core.common.Pose | None speed_factor: float tcp_offset_configured_in_desk: bool world_to_robot: rcs._core.common.Pose | None - def __init__(self) -> None: ... class FrankaHand(rcs._core.common.Gripper): - def __init__(self, ip: str, cfg: FHConfig) -> None: ... + def __init__(self, cfg: FHConfig) -> None: ... def close(self) -> None: ... def get_config(self) -> FHConfig: ... def get_state(self) -> FHState: ... @@ -284,10 +293,38 @@ class RobotState: def theta(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ... class FR3Config(FrankaConfig): - def __init__(self) -> None: ... + def __init__( + self, + ip: str, + ik_solver: IKSolver = ..., + speed_factor: float = 0.2, + load_parameters: FrankaLoad | None = None, + nominal_end_effector_frame: rcs._core.common.Pose | None = None, + world_to_robot: rcs._core.common.Pose | None = None, + async_control: bool = False, + tcp_offset_configured_in_desk: bool = True, + ignore_realtime: bool = False, + tcp_offset: rcs._core.common.Pose = ..., + attachment_site: str = "attachment_site", + kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml", + ) -> None: ... class PandaConfig(FrankaConfig): - def __init__(self) -> None: ... + def __init__( + self, + ip: str, + ik_solver: IKSolver = ..., + speed_factor: float = 0.2, + load_parameters: FrankaLoad | None = None, + nominal_end_effector_frame: rcs._core.common.Pose | None = None, + world_to_robot: rcs._core.common.Pose | None = None, + async_control: bool = False, + tcp_offset_configured_in_desk: bool = True, + ignore_realtime: bool = False, + tcp_offset: rcs._core.common.Pose = ..., + attachment_site: str = "attachment_site", + kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml", + ) -> None: ... class FrankaState(rcs._core.common.RobotState): def __init__(self) -> None: ... diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py index 569cf483..d59c1483 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -90,16 +90,17 @@ def __call__( # type: ignore ) # ik = FastIK # ik = rcs_robotics_library._core.rl.RoboticsLibraryIK(robot_cfg.kinematic_model_path) - robot = hw.Franka(ip, ik) - robot.set_config(robot_cfg) + robot_cfg.ip = ip + robot = hw.Franka(robot_cfg, ik) env: gym.Env = HardwareEnv() env = RobotWrapper(env, robot, ControlMode.JOINTS if collision_guard is not None else control_mode) env = FR3HW(env) if isinstance(gripper_cfg, hw.FHConfig): - gripper = hw.FrankaHand(ip, gripper_cfg) - env = GripperWrapper(env, gripper, binary=True) + gripper_cfg.ip = ip + gripper = hw.FrankaHand(gripper_cfg) + env = GripperWrapper(env, gripper) elif isinstance(gripper_cfg, rcs.hand.tilburg_hand.THConfig): hand = TilburgHand(gripper_cfg) env = HandWrapper(env, hand, binary=True) @@ -151,8 +152,8 @@ def __call__( # type: ignore robots: dict[str, hw.Franka] = {} for key, ip in name2ip.items(): - robots[key] = hw.Franka(ip, ik) - robots[key].set_config(robot_cfg) + robot_cfg.ip = ip + robots[key] = hw.Franka(robot_cfg, ik) envs: dict[str, gym.Env] = {} env: gym.Env @@ -161,8 +162,9 @@ def __call__( # type: ignore env = RobotWrapper(env, robots[key], control_mode) env = FR3HW(env) if gripper_cfg is not None: - gripper = hw.FrankaHand(ip, gripper_cfg) - env = GripperWrapper(env, gripper, binary=True) + gripper_cfg.ip = ip + gripper = hw.FrankaHand(gripper_cfg) + env = GripperWrapper(env, gripper) if max_relative_movement is not None: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) @@ -190,9 +192,9 @@ def __call__( # type: ignore ip=robot_ip, camera_set=camera_set, control_mode=control_mode, - robot_cfg=default_fr3_hw_robot_cfg(), + robot_cfg=default_fr3_hw_robot_cfg(robot_ip), collision_guard=None, - gripper_cfg=default_fr3_hw_gripper_cfg() if gripper else None, + gripper_cfg=default_fr3_hw_gripper_cfg(robot_ip) if gripper else None, max_relative_movement=(0.2, np.deg2rad(45)) if delta_actions else None, relative_to=RelativeTo.LAST_STEP, ) diff --git a/extensions/rcs_fr3/src/rcs_fr3/desk.py b/extensions/rcs_fr3/src/rcs_fr3/desk.py index d898b377..7e7289ec 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/desk.py +++ b/extensions/rcs_fr3/src/rcs_fr3/desk.py @@ -47,12 +47,11 @@ def load_creds_franka_desk(postfix: str = "") -> tuple[str, str]: def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False): with Desk.fci(ip, username, password, unlock=unlock): - f = rcs_fr3.hw.Franka(ip) - robot_cfg = default_fr3_hw_robot_cfg() + robot_cfg = default_fr3_hw_robot_cfg(ip) robot_cfg.speed_factor = 0.2 - f.set_config(robot_cfg) - config_hand = rcs_fr3.hw.FHConfig() - g = rcs_fr3.hw.FrankaHand(ip, config_hand) + f = rcs_fr3.hw.Franka(robot_cfg) + config_hand = rcs_fr3.hw.FHConfig(ip=ip) + g = rcs_fr3.hw.FrankaHand(config_hand) if shut: g.shut() else: @@ -62,18 +61,17 @@ def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False def info(ip: str, username: str, password: str, include_hand: bool = False): with Desk.fci(ip, username, password): - robot_cfg = rcs_fr3.hw.FR3Config() + robot_cfg = rcs_fr3.hw.FR3Config(ip=ip) robot_cfg.speed_factor = 0.2 - f = rcs_fr3.hw.Franka(ip) - f.set_config(robot_cfg) + f = rcs_fr3.hw.Franka(robot_cfg) print("Robot info:") print("Current cartesian position:") print(f.get_cartesian_position()) print("Current joint position:") print(f.get_joint_position()) if include_hand: - config_hand = default_fr3_hw_gripper_cfg() - g = rcs_fr3.hw.FrankaHand(ip, config_hand) + config_hand = default_fr3_hw_gripper_cfg(ip) + g = rcs_fr3.hw.FrankaHand(config_hand) print("Gripper info:") print("Current normalized width:") print(g.get_normalized_width()) diff --git a/extensions/rcs_fr3/src/rcs_fr3/utils.py b/extensions/rcs_fr3/src/rcs_fr3/utils.py index 0729a918..679d8d81 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/utils.py +++ b/extensions/rcs_fr3/src/rcs_fr3/utils.py @@ -4,8 +4,8 @@ from rcs import common -def default_fr3_hw_robot_cfg(async_control: bool = False) -> hw.FR3Config: - robot_cfg = hw.FR3Config() +def default_fr3_hw_robot_cfg(ip: str = "", async_control: bool = False) -> hw.FR3Config: + robot_cfg = hw.FR3Config(ip=ip) robot_cfg.robot_type = rcs.scenes["fr3_empty_world"].robot_type robot_cfg.kinematic_model_path = rcs.scenes["fr3_empty_world"].mjcf_robot robot_cfg.tcp_offset = common.Pose(common.FrankaHandTCPOffset()) @@ -16,8 +16,8 @@ def default_fr3_hw_robot_cfg(async_control: bool = False) -> hw.FR3Config: return robot_cfg -def default_fr3_hw_gripper_cfg(async_control: bool = False) -> hw.FHConfig: - gripper_cfg = hw.FHConfig() +def default_fr3_hw_gripper_cfg(ip: str = "", async_control: bool = False) -> hw.FHConfig: + gripper_cfg = hw.FHConfig(ip=ip) gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1 gripper_cfg.speed = 0.1 gripper_cfg.force = 30 diff --git a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi index 3c1c9082..0af7ab19 100644 --- a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi +++ b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi @@ -43,8 +43,18 @@ class FHConfig(rcs._core.common.GripperConfig): epsilon_outer: float force: float grasping_width: float + ip: str speed: float - def __init__(self) -> None: ... + def __init__( + self, + ip: str, + grasping_width: float = 0.05, + speed: float = 0.1, + force: float = 5.0, + epsilon_inner: float = 0.005, + epsilon_outer: float = 0.005, + async_control: bool = False, + ) -> None: ... class FHState(rcs._core.common.GripperState): def __init__(self) -> None: ... @@ -64,9 +74,7 @@ class FHState(rcs._core.common.GripperState): def width(self) -> float: ... class Franka(rcs._core.common.Robot): - def __init__( - self, ip: str, ik: rcs._core.common.Kinematics | None = None, cfg: FrankaConfig | None = None - ) -> None: ... + def __init__(self, cfg: FrankaConfig, ik: rcs._core.common.Kinematics | None = None) -> None: ... def automatic_error_recovery(self) -> None: ... def controller_set_joint_position( self, desired_q: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] @@ -96,16 +104,17 @@ class Franka(rcs._core.common.Robot): class FrankaConfig(rcs._core.common.RobotConfig): async_control: bool + ignore_realtime: bool ik_solver: IKSolver + ip: str load_parameters: FrankaLoad | None nominal_end_effector_frame: rcs._core.common.Pose | None speed_factor: float tcp_offset_configured_in_desk: bool world_to_robot: rcs._core.common.Pose | None - def __init__(self) -> None: ... class FrankaHand(rcs._core.common.Gripper): - def __init__(self, ip: str, cfg: FHConfig) -> None: ... + def __init__(self, cfg: FHConfig) -> None: ... def close(self) -> None: ... def get_config(self) -> FHConfig: ... def get_state(self) -> FHState: ... @@ -284,10 +293,38 @@ class RobotState: def theta(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(7)]: ... class FR3Config(FrankaConfig): - def __init__(self) -> None: ... + def __init__( + self, + ip: str, + ik_solver: IKSolver = ..., + speed_factor: float = 0.2, + load_parameters: FrankaLoad | None = None, + nominal_end_effector_frame: rcs._core.common.Pose | None = None, + world_to_robot: rcs._core.common.Pose | None = None, + async_control: bool = False, + tcp_offset_configured_in_desk: bool = True, + ignore_realtime: bool = False, + tcp_offset: rcs._core.common.Pose = ..., + attachment_site: str = "attachment_site", + kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml", + ) -> None: ... class PandaConfig(FrankaConfig): - def __init__(self) -> None: ... + def __init__( + self, + ip: str, + ik_solver: IKSolver = ..., + speed_factor: float = 0.2, + load_parameters: FrankaLoad | None = None, + nominal_end_effector_frame: rcs._core.common.Pose | None = None, + world_to_robot: rcs._core.common.Pose | None = None, + async_control: bool = False, + tcp_offset_configured_in_desk: bool = True, + ignore_realtime: bool = False, + tcp_offset: rcs._core.common.Pose = ..., + attachment_site: str = "attachment_site", + kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml", + ) -> None: ... class FrankaState(rcs._core.common.RobotState): def __init__(self) -> None: ... diff --git a/extensions/rcs_panda/src/rcs_panda/creators.py b/extensions/rcs_panda/src/rcs_panda/creators.py index b5d452ee..2936b346 100644 --- a/extensions/rcs_panda/src/rcs_panda/creators.py +++ b/extensions/rcs_panda/src/rcs_panda/creators.py @@ -67,21 +67,21 @@ def __call__( # type: ignore ) # ik = rcs_robotics_library._core.rl.RoboticsLibraryIK(robot_cfg.kinematic_model_path) - robot = hw.Franka(ip, ik) - robot.set_config(robot_cfg) + robot_cfg.ip = ip + robot = hw.Franka(robot_cfg, ik) env: gym.Env = HardwareEnv() env = RobotWrapper( env, robot, ControlMode.JOINTS if collision_guard is not None else control_mode, - home_on_reset=True, ) env = PandaHW(env) if isinstance(gripper_cfg, hw.FHConfig): - gripper = hw.FrankaHand(ip, gripper_cfg) - env = GripperWrapper(env, gripper, binary=True) + gripper_cfg.ip = ip + gripper = hw.FrankaHand(gripper_cfg) + env = GripperWrapper(env, gripper) elif isinstance(gripper_cfg, rcs.hand.tilburg_hand.THConfig): hand = TilburgHand(gripper_cfg) env = HandWrapper(env, hand, binary=True) @@ -131,8 +131,8 @@ def __call__( # type: ignore robots: dict[str, hw.Franka] = {} for ip in ips: - robots[ip] = hw.Franka(ip, ik) - robots[ip].set_config(robot_cfg) + robot_cfg.ip = ip + robots[ip] = hw.Franka(robot_cfg, ik) envs: dict[str, gym.Env] = {} env: gym.Env @@ -141,8 +141,9 @@ def __call__( # type: ignore env = RobotWrapper(env, robots[ip], control_mode) env = PandaHW(env) if gripper_cfg is not None: - gripper = hw.FrankaHand(ip, gripper_cfg) - env = GripperWrapper(env, gripper, binary=True) + gripper_cfg.ip = ip + gripper = hw.FrankaHand(gripper_cfg) + env = GripperWrapper(env, gripper) if max_relative_movement is not None: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) diff --git a/extensions/rcs_panda/src/rcs_panda/desk.py b/extensions/rcs_panda/src/rcs_panda/desk.py index 5d159f5a..7593250d 100644 --- a/extensions/rcs_panda/src/rcs_panda/desk.py +++ b/extensions/rcs_panda/src/rcs_panda/desk.py @@ -47,12 +47,11 @@ def load_creds_franka_desk(postfix: str = "") -> tuple[str, str]: def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False): with Desk.fci(ip, username, password, unlock=unlock): - robot_cfg = default_panda_hw_robot_cfg() + robot_cfg = default_panda_hw_robot_cfg(ip) robot_cfg.speed_factor = 0.2 - f = rcs_panda.hw.Franka(ip) - f.set_config(robot_cfg) - config_hand = rcs_panda.hw.FHConfig() - g = rcs_panda.hw.FrankaHand(ip, config_hand) + f = rcs_panda.hw.Franka(robot_cfg) + config_hand = rcs_panda.hw.FHConfig(ip=ip) + g = rcs_panda.hw.FrankaHand(config_hand) if shut: g.shut() else: @@ -62,18 +61,17 @@ def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False def info(ip: str, username: str, password: str, include_hand: bool = False): with Desk.fci(ip, username, password): - robot_cfg = rcs_panda.hw.PandaConfig() + robot_cfg = rcs_panda.hw.PandaConfig(ip=ip) robot_cfg.speed_factor = 0.2 - f = rcs_panda.hw.Franka(ip) - f.set_config(robot_cfg) + f = rcs_panda.hw.Franka(robot_cfg) print("Robot info:") print("Current cartesian position:") print(f.get_cartesian_position()) print("Current joint position:") print(f.get_joint_position()) if include_hand: - config_hand = default_panda_hw_gripper_cfg() - g = rcs_panda.hw.FrankaHand(ip, config_hand) + config_hand = default_panda_hw_gripper_cfg(ip) + g = rcs_panda.hw.FrankaHand(config_hand) print("Gripper info:") print("Current normalized width:") print(g.get_normalized_width()) diff --git a/extensions/rcs_panda/src/rcs_panda/utils.py b/extensions/rcs_panda/src/rcs_panda/utils.py index eacaed79..c9981c89 100644 --- a/extensions/rcs_panda/src/rcs_panda/utils.py +++ b/extensions/rcs_panda/src/rcs_panda/utils.py @@ -4,8 +4,8 @@ from rcs import common -def default_panda_hw_robot_cfg(async_control: bool = False) -> hw.PandaConfig: - robot_cfg = hw.PandaConfig() +def default_panda_hw_robot_cfg(ip: str = "", async_control: bool = False) -> hw.PandaConfig: + robot_cfg = hw.PandaConfig(ip=ip) robot_cfg.robot_type = rcs.scenes["panda_empty_world"].robot_type robot_cfg.kinematic_model_path = rcs.scenes["panda_empty_world"].mjcf_robot robot_cfg.tcp_offset = common.Pose(common.FrankaHandTCPOffset()) @@ -16,8 +16,8 @@ def default_panda_hw_robot_cfg(async_control: bool = False) -> hw.PandaConfig: return robot_cfg -def default_panda_hw_gripper_cfg(async_control: bool = False) -> hw.FHConfig: - gripper_cfg = hw.FHConfig() +def default_panda_hw_gripper_cfg(ip: str = "", async_control: bool = False) -> hw.FHConfig: + gripper_cfg = hw.FHConfig(ip=ip) gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1 gripper_cfg.speed = 0.1 gripper_cfg.force = 30 diff --git a/extensions/rcs_robotiq2f85/README.md b/extensions/rcs_robotiq2f85/README.md index baa44c9c..3522d8ca 100644 --- a/extensions/rcs_robotiq2f85/README.md +++ b/extensions/rcs_robotiq2f85/README.md @@ -18,9 +18,9 @@ chmod 777 /dev/ttyUSB0 ## Usage ```python -from rcs_robotiq2f85 import RobotiQGripper +from rcs_robotiq2f85 import RobotiQGripper, RobotiQ2F85GripperConfig -gripper = RobotiQGripper('') +gripper = RobotiQGripper(RobotiQ2F85GripperConfig(serial_number='')) gripper.reset() gripper.shut() print(gripper.get_normalized_width()) diff --git a/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py b/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py index 5e5ee766..9fbe047c 100644 --- a/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py +++ b/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py @@ -1,37 +1,45 @@ -from dataclasses import dataclass +import typing from rcs._core.common import Gripper, GripperConfig, GripperState +from rcs.common_typing import GripperConfigKwargs from Robotiq2F85Driver.Robotiq2F85Driver import GripperStatus, Robotiq2F85Driver -@dataclass class RobotiQ2F85GripperConfig(GripperConfig): - speed: float = 100 - """Speed in mm/s. Must be between 20 and 150 mm/s.""" - force: float = 50 - """Force in N. Must be between 20 and 235 N.""" - async_control: bool = True - """If True, gripper commands return immediately without waiting for the movement to complete. - A new command interrupts any ongoing movement.""" + def __init__( + self, + serial_number: str, + speed: float = 100, + force: float = 50, + async_control: bool = True, + **kwargs: typing.Unpack[GripperConfigKwargs], + ) -> None: + """ + Args: + serial_number: Get the serial number with `udevadm info -a -n /dev/ttyUSB0 | grep serial`, make sure you have read/write permissions to the port. + speed: Speed in mm/s. Must be between 20 and 150 mm/s. + force: Force in N. Must be between 20 and 235 N. + async_control: If True, gripper commands return immediately without waiting for the movement to complete. A new command interrupts any ongoing movement. + """ + super().__init__(**kwargs) + self.serial_number = serial_number + self.speed = speed + self.force = force + self.async_control = async_control -@dataclass(kw_only=True) -class RobotiQ2F85GripperState(GripperState): - state: GripperStatus - def __post_init__(self): +class RobotiQ2F85GripperState(GripperState): + def __init__(self, state: GripperStatus) -> None: super().__init__() + self.state = state class RobotiQ2F85Gripper(Gripper): - def __init__(self, serial_number: str, cfg: RobotiQ2F85GripperConfig): - """ - serial_number: - Get the serial number with `udevadm info -a -n /dev/ttyUSB0 | grep serial`, make sure you have read/write permissions to the port. - """ + def __init__(self, cfg: RobotiQ2F85GripperConfig): super().__init__() self._cfg: RobotiQ2F85GripperConfig = cfg - self.gripper = Robotiq2F85Driver(serial_number=serial_number) + self.gripper = Robotiq2F85Driver(serial_number=cfg.serial_number) def get_normalized_width(self) -> float: # value between 0 and 1 (0 is closed) diff --git a/extensions/rcs_so101/src/rcs_so101/creators.py b/extensions/rcs_so101/src/rcs_so101/creators.py index 6c541bec..90f850db 100644 --- a/extensions/rcs_so101/src/rcs_so101/creators.py +++ b/extensions/rcs_so101/src/rcs_so101/creators.py @@ -34,12 +34,12 @@ def __call__( # type: ignore robot_cfg.attachment_site, urdf=robot_cfg.kinematic_model_path.endswith(".urdf"), ) - robot = SO101(robot_cfg=robot_cfg, ik=ik) + robot = SO101(cfg=robot_cfg, ik=ik) env: gym.Env = HardwareEnv() - env = RobotWrapper(env, robot, control_mode, home_on_reset=True) + env = RobotWrapper(env, robot, control_mode) gripper = SO101Gripper(robot._hf_robot, robot) - env = GripperWrapper(env, gripper, binary=False) + env = GripperWrapper(env, gripper) if camera_set is not None: camera_set.start() diff --git a/extensions/rcs_so101/src/rcs_so101/hw.py b/extensions/rcs_so101/src/rcs_so101/hw.py index 4ab55619..0362d779 100644 --- a/extensions/rcs_so101/src/rcs_so101/hw.py +++ b/extensions/rcs_so101/src/rcs_so101/hw.py @@ -6,25 +6,39 @@ from lerobot.robots import make_robot_from_config from lerobot.robots.so101_follower.config_so101_follower import SO101FollowerConfig from lerobot.robots.so101_follower.so101_follower import SO101Follower +from rcs.common_typing import RobotConfigKwargs from rcs.utils import SimpleFrameRate from rcs import common class SO101Config(common.RobotConfig): - id: str = "follower" - port: str = "/dev/ttyACM0" - calibration_dir: str = "." + + def __init__( + self, + id: str = "follower", + port: str = "/dev/ttyACM0", + calibration_dir: str = ".", + **kwargs: typing.Unpack[RobotConfigKwargs], + ): + super().__init__(**kwargs) + self.id = id + self.port = port + self.calibration_dir = calibration_dir class SO101(common.Robot): - def __init__(self, robot_cfg: SO101Config, ik: common.Kinematics): + def __init__(self, cfg: SO101Config, ik: common.Kinematics): super().__init__() self.ik = ik - cfg = SO101FollowerConfig(id=robot_cfg.id, calibration_dir=Path(robot_cfg.calibration_dir), port=robot_cfg.port) + self._robot_config = cfg + cfg = SO101FollowerConfig( + id=self._robot_config.id, + calibration_dir=Path(self._robot_config.calibration_dir), + port=self._robot_config.port, + ) self._hf_robot = make_robot_from_config(cfg) self._hf_robot.connect() - self._robot_config = robot_cfg self._thread: threading.Thread | None = None self._running = False self._goal = None @@ -174,6 +188,7 @@ def __init__(self, hf_robot: SO101Follower, robot: SO101): super().__init__() self._hf_robot = hf_robot self._robot = robot + self._cfg = common.GripperConfig(binary=False) def get_normalized_width(self) -> float: obs = self._robot.obs @@ -181,7 +196,9 @@ def get_normalized_width(self) -> float: return 0.0 return obs["gripper.pos"] / 100.0 - # def get_config(self) -> GripperConfig: ... + def get_config(self) -> common.GripperConfig: + return self._cfg + # def get_state(self) -> GripperState: ... def grasp(self) -> None: diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/creators.py b/extensions/rcs_ur5e/src/rcs_ur5e/creators.py index 3cc6e2c0..a0346c56 100644 --- a/extensions/rcs_ur5e/src/rcs_ur5e/creators.py +++ b/extensions/rcs_ur5e/src/rcs_ur5e/creators.py @@ -13,7 +13,7 @@ RobotWrapper, ) from rcs.envs.creators import RCSHardwareEnvCreator -from rcs_ur5e.hw import RobotiQGripper, UR5e, UR5eConfig +from rcs_ur5e.hw import RobotiQGripper, RobotiQGripperConfig, UR5e, UR5eConfig import rcs @@ -24,8 +24,8 @@ class RCSUR5eEnvCreator(RCSHardwareEnvCreator): def __call__( # type: ignore self, - ip: str, robot_cfg: UR5eConfig, + gripper_cfg: RobotiQGripperConfig | None = None, camera_set: HardwareCameraSet | None = None, control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, max_relative_movement: float | tuple[float, float] | None = None, @@ -36,13 +36,14 @@ def __call__( # type: ignore robot_cfg.attachment_site, urdf=robot_cfg.kinematic_model_path.endswith(".urdf"), ) - robot = UR5e(ip, ik) - robot.set_config(robot_cfg) + robot = UR5e(robot_cfg, ik) env: gym.Env = HardwareEnv() - env = RobotWrapper(env, robot, control_mode, home_on_reset=True) + env = RobotWrapper(env, robot, control_mode) - gripper = RobotiQGripper(ip) - env = GripperWrapper(env, gripper, binary=True) + if gripper_cfg is not None: + gripper = RobotiQGripper(cfg=gripper_cfg) + # TODO: binary and other things of the wrappers should also be in the config + env = GripperWrapper(env, gripper) if camera_set is not None: camera_set.start() diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/hw.py b/extensions/rcs_ur5e/src/rcs_ur5e/hw.py index 8aa1aa35..88402ff5 100644 --- a/extensions/rcs_ur5e/src/rcs_ur5e/hw.py +++ b/extensions/rcs_ur5e/src/rcs_ur5e/hw.py @@ -5,36 +5,45 @@ import multiprocessing as mp import time import typing -from dataclasses import dataclass from enum import IntEnum from multiprocessing.shared_memory import SharedMemory import numpy as np import rtde_control import rtde_receive +from rcs.common_typing import GripperConfigKwargs, RobotConfigKwargs from rcs_ur5e import robotiq_gripper -import rcs from rcs import common -@dataclass(kw_only=True) class UR5eConfig(common.RobotConfig): - # Kinematics Setup - robot_type: common.RobotType = common.RobotType.UR5e - kinematic_model_path = rcs.scenes["ur5e_empty_world"].mjcf_robot - attachment_site = "attachment_site" - - # Robot movement parameters - max_velocity: float = 1.0 - max_acceleration: float = 1.0 - async_control: bool = True - max_servo_joint_step: float = 0.15 - max_servo_cartesian_step: float = 0.01 - - # UR Controller parameters, change with caution - lookahead_time: float = 0.05 - gain: float = 500.0 + + def __init__( + self, + ip: str, + max_velocity: float = 1.0, + max_acceleration: float = 1.0, + async_control: bool = True, + max_servo_joint_step: float = 0.15, + max_servo_cartesian_step: float = 0.01, + lookahead_time: float = 0.05, + gain: float = 500.0, + **kwargs: typing.Unpack[RobotConfigKwargs], + ): + super().__init__(**kwargs) + self.robot_platform = common.RobotPlatform.HARDWARE + self.robot_type = common.RobotType.UR5e + self.ip = ip + # Robot movement parameters + self.max_velocity = max_velocity + self.max_acceleration = max_acceleration + self.async_control = async_control + self.max_servo_joint_step = max_servo_joint_step + self.max_servo_cartesian_step = max_servo_cartesian_step + # UR Controller parameters, change with caution + self.lookahead_time = lookahead_time + self.gain = gain def to_dict(self) -> dict[str, typing.Any]: return { @@ -53,8 +62,11 @@ def from_dict(self, data: dict[str, typing.Any]) -> None: for key, value in data.items(): setattr(self, key, value) - def __post_init__(self): - super().__init__() + +class RobotiQGripperConfig(common.GripperConfig): + def __init__(self, ip: str, **kwargs: typing.Unpack[GripperConfigKwargs]) -> None: + super().__init__(**kwargs) + self.ip = ip # Define the shared memory @@ -72,7 +84,7 @@ def _control_robot(shm_name: str, ip: str, stop_queue: mp.Queue, config_queue: m """ Control loop for the robot, running in a separate process. """ - robot_config = UR5eConfig() + robot_config = UR5eConfig(ip=ip) try: # Initialize robot interfaces ur_control = rtde_control.RTDEControlInterface(ip) @@ -178,12 +190,12 @@ def _control_robot(shm_name: str, ip: str, stop_queue: mp.Queue, config_queue: m class UR5e(common.Robot): - def __init__(self, ip: str, ik: common.Kinematics): + def __init__(self, cfg: UR5eConfig, ik: common.Kinematics): super().__init__() self.ik = ik - self._config = UR5eConfig() + self._config = cfg self._config.robot_type = common.RobotType.UR5e - self._ip = ip + self._ip = cfg.ip # Delete shared memory if it exists try: @@ -312,18 +324,22 @@ def reset(self) -> None: class RobotiQGripper(common.Gripper): - def __init__(self, ip): + def __init__(self, cfg: RobotiQGripperConfig): super().__init__() + self._cfg = cfg self.gripper = robotiq_gripper.RobotiqGripper() try: - self.gripper.connect(ip, 63352, socket_timeout=3.0) # default port for Robotiq gripper + self.gripper.connect(cfg.ip, 63352, socket_timeout=3.0) # default port for Robotiq gripper except Exception as e: - msg = f"Failed to connect to RobotiQ gripper at {ip}" + msg = f"Failed to connect to RobotiQ gripper at {cfg.ip}" raise RuntimeError(msg) from e if not self.gripper.is_active(): self.gripper.activate() print("Gripper Connection established.") + def get_config(self) -> RobotiQGripperConfig: + return self._cfg + def get_normalized_width(self) -> float: # value between 0 and 1 (0 is closed) return (self.gripper.get_max_position() - self.gripper.get_current_position()) / self.gripper.get_max_position() @@ -343,7 +359,7 @@ def open(self) -> None: def reset(self) -> None: self.open() - def set_normalized_width(self, width: float, _: float = 0) -> None: + def set_normalized_width(self, width: float, force: float = 0) -> None: """ Set the gripper width to a normalized value between 0 and 1. """ diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/scripts/test_gripper.py b/extensions/rcs_ur5e/src/rcs_ur5e/scripts/test_gripper.py index c40195b2..9fd7b639 100644 --- a/extensions/rcs_ur5e/src/rcs_ur5e/scripts/test_gripper.py +++ b/extensions/rcs_ur5e/src/rcs_ur5e/scripts/test_gripper.py @@ -1,10 +1,10 @@ """Test script for the RobotiQ gripper.""" -from rcs_ur5e.hw import RobotiQGripper +from rcs_ur5e.hw import RobotiQGripper, RobotiQGripperConfig ROBOT_IP = "192.168.25.201" -gripper = RobotiQGripper(ROBOT_IP) +gripper = RobotiQGripper(RobotiQGripperConfig(ip=ROBOT_IP)) gripper.open() print(f"Gripper width: {gripper.get_normalized_width()}") diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/scripts/test_robot.py b/extensions/rcs_ur5e/src/rcs_ur5e/scripts/test_robot.py index 7cd0fd3d..6c58b37b 100644 --- a/extensions/rcs_ur5e/src/rcs_ur5e/scripts/test_robot.py +++ b/extensions/rcs_ur5e/src/rcs_ur5e/scripts/test_robot.py @@ -6,14 +6,14 @@ import rcs -robot_config = UR5eConfig() ROBOT_IP = "192.168.25.201" +robot_config = UR5eConfig(ip=ROBOT_IP) ik = rcs.common.Pin( robot_config.kinematic_model_path, robot_config.attachment_site, urdf=robot_config.kinematic_model_path.endswith(".urdf"), ) -robot = UR5e(ROBOT_IP, ik) +robot = UR5e(robot_config, ik) print(f"Robot joint positions: {robot.get_joint_position()}") print(f"Robot cartesian position: {robot.get_cartesian_position()}") print(f"Robot Config: {robot.get_config()}") diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py index 9a97b3b9..d7bb336c 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py @@ -16,7 +16,9 @@ ) from rcs.envs.creators import RCSHardwareEnvCreator from rcs.hand.tilburg_hand import THConfig, TilburgHand -from rcs_xarm7.hw import XArm7 +from rcs_xarm7.hw import XArm7, XArm7Config + +import rcs logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -25,8 +27,8 @@ class RCSXArm7EnvCreator(RCSHardwareEnvCreator): def __call__( # type: ignore self, + robot_cfg: XArm7Config, control_mode: ControlMode, - ip: str, calibration_dir: PathLike | str | None = None, camera_set: HardwareCameraSet | None = None, hand_cfg: THConfig | None = None, @@ -35,9 +37,14 @@ def __call__( # type: ignore ) -> gym.Env: if isinstance(calibration_dir, str): calibration_dir = Path(calibration_dir) - robot = XArm7(ip=ip) + ik = rcs.common.Pin( + robot_cfg.kinematic_model_path, + robot_cfg.attachment_site, + urdf=robot_cfg.kinematic_model_path.endswith(".urdf"), + ) + robot = XArm7(cfg=robot_cfg, ik=ik) env: gym.Env = HardwareEnv() - env = RobotWrapper(env, robot, control_mode, home_on_reset=True) + env = RobotWrapper(env, robot, control_mode) if camera_set is not None: camera_set.start() diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py b/extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py index 350b0295..4610eef0 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py @@ -8,6 +8,7 @@ from rcs.envs.utils import default_sim_tilburg_hand_cfg from rcs.hand.tilburg_hand import THConfig from rcs_xarm7.creators import RCSXArm7EnvCreator +from rcs_xarm7.hw import XArm7Config import rcs from rcs import sim @@ -63,9 +64,10 @@ def main(): hand_cfg = THConfig( calibration_file="/home/ken/tilburg_hand/calibration.json", grasp_percentage=1, hand_orientation="right" ) + robot_cfg = XArm7Config(ip=ROBOT_IP) env_rel = RCSXArm7EnvCreator()( + robot_cfg=robot_cfg, control_mode=ControlMode.JOINTS, - ip=ROBOT_IP, hand_cfg=hand_cfg, relative_to=RelativeTo.LAST_STEP, max_relative_movement=None, diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/hw.py b/extensions/rcs_xarm7/src/rcs_xarm7/hw.py index 2fabff45..d1a46d66 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/hw.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/hw.py @@ -1,33 +1,42 @@ import typing -from dataclasses import dataclass, field from typing import List import numpy as np +from rcs.common_typing import RobotConfigKwargs from xarm.wrapper import XArmAPI from rcs import common -@dataclass(kw_only=True) class XArm7Config(common.RobotConfig): - payload_weight: float = 0.624 - payload_tcp: List[float] = field(default_factory=lambda: [-4.15, 5.24, 76.38]) - async_control: bool = False - def __post_init__(self): - super().__init__() + def __init__( + self, + ip: str, + payload_weight: float = 0.624, + payload_tcp: List[float] | None = None, + async_control: bool = False, + use_internal_ik: bool = True, + **kwargs: typing.Unpack[RobotConfigKwargs], + ): + super().__init__(**kwargs) + self.ip = ip + self.payload_weight = payload_weight + self.payload_tcp = payload_tcp if payload_tcp is not None else [-4.15, 5.24, 76.38] + self.async_control = async_control + self.use_internal_ik = use_internal_ik class XArm7(common.Robot): - def __init__(self, ip: str): + def __init__(self, cfg: XArm7Config, ik: common.Kinematics): super().__init__() - self.ik = None - self._config = XArm7Config() + self.ik = ik + self._config = cfg self._config.robot_platform = common.RobotPlatform.HARDWARE self._config.robot_type = common.RobotType.XArm7 - self._xarm = XArmAPI(ip) + self._xarm = XArmAPI(cfg.ip) self._xarm.set_mode(0) self._xarm.clean_error() self._xarm.clean_warn() @@ -83,6 +92,11 @@ def reset(self) -> None: pass def set_cartesian_position(self, pose: common.Pose) -> None: + if not self._config.use_internal_ik: + target_joints = self.ik.inverse(pose=pose, q0=self.get_joint_position(), tcp_offset=self._config.tcp_offset) + if target_joints is not None: + self.set_joint_position(target_joints) + return if self._config.async_control: self._xarm.set_mode(7) self._xarm.set_state(0) diff --git a/include/rcs/Camera.h b/include/rcs/Camera.h index 8ecc98b9..2d7aac35 100644 --- a/include/rcs/Camera.h +++ b/include/rcs/Camera.h @@ -10,13 +10,7 @@ struct BaseCameraConfig { int frame_rate; int resolution_width; int resolution_height; - BaseCameraConfig(const std::string &identifier, int frame_rate, - int resolution_width, int resolution_height) - : identifier(identifier), - frame_rate(frame_rate), - resolution_width(resolution_width), - resolution_height(resolution_height) {} - ~BaseCameraConfig() {} + virtual ~BaseCameraConfig() {} }; } // namespace common diff --git a/include/rcs/Robot.h b/include/rcs/Robot.h index bda901e7..867fdb3a 100644 --- a/include/rcs/Robot.h +++ b/include/rcs/Robot.h @@ -121,6 +121,8 @@ struct RobotConfig { rcs::common::Pose tcp_offset = rcs::common::Pose::Identity(); std::string attachment_site = "attachment_site"; std::string kinematic_model_path = "assets/scenes/fr3_empty_world/robot.xml"; + bool home_on_reset = true; + std::optional q_home = std::nullopt; virtual ~RobotConfig(){}; }; struct RobotState { @@ -128,6 +130,7 @@ struct RobotState { }; struct GripperConfig { + bool binary = true; virtual ~GripperConfig(){}; }; struct GripperState { diff --git a/pyproject.toml b/pyproject.toml index d57bd7ca..b4f00608 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -131,6 +131,8 @@ ignore = [ "G004", # Logging format string does not contain any placeholders "ARG002", ] +[tool.ruff.lint.per-file-ignores] +"python/rcs/_core/*.pyi" = ["B006"] [tool.pylint.format] max-line-length = 120 diff --git a/python/rcs/_core/common.pyi b/python/rcs/_core/common.pyi index a120cd07..7be893ac 100644 --- a/python/rcs/_core/common.pyi +++ b/python/rcs/_core/common.pyi @@ -54,6 +54,8 @@ class BaseCameraConfig: identifier: str resolution_height: int resolution_width: int + def __copy__(self) -> BaseCameraConfig: ... + def __deepcopy__(self, arg0: dict) -> BaseCameraConfig: ... def __init__(self, identifier: str, frame_rate: int, resolution_width: int, resolution_height: int) -> None: ... class GraspType: @@ -105,7 +107,8 @@ class Gripper: def shut(self) -> None: ... class GripperConfig: - def __init__(self) -> None: ... + binary: bool + def __init__(self, binary: bool = True) -> None: ... class GripperState: def __init__(self) -> None: ... @@ -232,11 +235,22 @@ class Robot: class RobotConfig: attachment_site: str + home_on_reset: bool kinematic_model_path: str + q_home: numpy.ndarray | None robot_platform: RobotPlatform robot_type: RobotType tcp_offset: Pose - def __init__(self) -> None: ... + def __init__( + self, + robot_type: RobotType = ..., + robot_platform: RobotPlatform = ..., + tcp_offset: Pose = ..., + attachment_site: str = "attachment_site", + kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml", + home_on_reset: bool = True, + q_home: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]] | None = None, + ) -> None: ... class RobotMetaConfig: @property diff --git a/python/rcs/_core/sim.pyi b/python/rcs/_core/sim.pyi index 334bd677..a75d598a 100644 --- a/python/rcs/_core/sim.pyi +++ b/python/rcs/_core/sim.pyi @@ -69,7 +69,12 @@ class CameraType: def value(self) -> int: ... class FrameSet: - def __init__(self) -> None: ... + def __init__( + self, + color_frames: dict[str, numpy.ndarray[tuple[M], numpy.dtype[numpy.uint8]]], + depth_frames: dict[str, numpy.ndarray[tuple[M], numpy.dtype[numpy.float32]]], + timestamp: float, + ) -> None: ... @property def color_frames(self) -> dict[str, numpy.ndarray[tuple[M], numpy.dtype[numpy.uint8]]]: ... @property @@ -114,7 +119,15 @@ class SimConfig: frequency: int max_convergence_steps: int realtime: bool - def __init__(self) -> None: ... + def __copy__(self) -> SimConfig: ... + def __deepcopy__(self, arg0: dict) -> SimConfig: ... + def __init__( + self, + async_control: bool = False, + realtime: bool = False, + frequency: float = 30, + max_convergence_steps: int = 500, + ) -> None: ... class SimGripper(rcs._core.common.Gripper): def __init__(self, sim: Sim, cfg: SimGripperConfig) -> None: ... @@ -138,7 +151,21 @@ class SimGripperConfig(rcs._core.common.GripperConfig): seconds_between_callbacks: float def __copy__(self) -> SimGripperConfig: ... def __deepcopy__(self, arg0: dict) -> SimGripperConfig: ... - def __init__(self) -> None: ... + def __init__( + self, + epsilon_inner: float = 0.005, + epsilon_outer: float = 0.005, + seconds_between_callbacks: float = 0.05, + ignored_collision_geoms: list[str] = [], + collision_geoms: list[str] = ["hand_c", "d435i_collision", "finger_0_left", "finger_0_right"], + collision_geoms_fingers: list[str] = ["finger_0_left", "finger_0_right"], + joints: list[str] = ["finger_joint1", "finger_joint2"], + max_joint_width: float = 0.04, + min_joint_width: float = 0.0, + actuator: str = "actuator8", + max_actuator_width: float = 255.0, + min_actuator_width: float = 0.0, + ) -> None: ... def add_postfix(self, id: str) -> None: ... class SimGripperState(rcs._core.common.GripperState): @@ -173,7 +200,46 @@ class SimRobotConfig(rcs._core.common.RobotConfig): trajectory_trace: bool def __copy__(self) -> SimRobotConfig: ... def __deepcopy__(self, arg0: dict) -> SimRobotConfig: ... - def __init__(self) -> None: ... + def __init__( + self, + robot_type: rcs._core.common.RobotType = ..., + tcp_offset: rcs._core.common.Pose = ..., + attachment_site: str = "attachment_site", + kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml", + joint_rotational_tolerance: float = 0.0008726646259971648, + seconds_between_callbacks: float = 0.1, + mjcf_scene_path: str = "assets/scenes/fr3_empty_world/scene.xml", + trajectory_trace: bool = False, + arm_collision_geoms: list[str] = [ + "fr3_link0_collision", + "fr3_link1_collision", + "fr3_link2_collision", + "fr3_link3_collision", + "fr3_link4_collision", + "fr3_link5_collision", + "fr3_link6_collision", + "fr3_link7_collision", + ], + joints: list[str] = [ + "fr3_joint1", + "fr3_joint2", + "fr3_joint3", + "fr3_joint4", + "fr3_joint5", + "fr3_joint6", + "fr3_joint7", + ], + actuators: list[str] = [ + "fr3_joint1", + "fr3_joint2", + "fr3_joint3", + "fr3_joint4", + "fr3_joint5", + "fr3_joint6", + "fr3_joint7", + ], + base: str = "base", + ) -> None: ... def add_postfix(self, id: str) -> None: ... class SimRobotState(rcs._core.common.RobotState): @@ -209,7 +275,54 @@ class SimTilburgHandConfig(rcs._core.common.HandConfig): max_joint_position: numpy.ndarray[tuple[typing.Literal[16]], numpy.dtype[numpy.float64]] min_joint_position: numpy.ndarray[tuple[typing.Literal[16]], numpy.dtype[numpy.float64]] seconds_between_callbacks: float - def __init__(self) -> None: ... + def __copy__(self) -> SimTilburgHandConfig: ... + def __deepcopy__(self, arg0: dict) -> SimTilburgHandConfig: ... + def __init__( + self, + grasp_type: rcs._core.common.GraspType = ..., + seconds_between_callbacks: float = 0.0167, + ignored_collision_geoms: list[str] = [], + collision_geoms: list[str] = [], + collision_geoms_fingers: list[str] = [], + joints: list[str] = [ + "thumb_ip", + "thumb_mcp", + "thumb_mcp_rot", + "thumb_cmc", + "index_dip", + "index_pip", + "index_mcp", + "index_mcp_abadd", + "middle_dip", + "middle_pip", + "middle_mcp", + "middle_mcp_abadd", + "ring_dip", + "ring_pip", + "ring_mcp", + "ring_mcp_abadd", + ], + actuators: list[str] = [ + "thumb_ip", + "thumb_mcp", + "thumb_mcp_rot", + "thumb_cmc", + "index_dip", + "index_pip", + "index_mcp", + "index_mcp_abadd", + "middle_dip", + "middle_pip", + "middle_mcp", + "middle_mcp_abadd", + "ring_dip", + "ring_pip", + "ring_mcp", + "ring_mcp_abadd", + ], + max_joint_position: numpy.ndarray[tuple[typing.Literal[16]], numpy.dtype[numpy.float64]] = ..., + min_joint_position: numpy.ndarray[tuple[typing.Literal[16]], numpy.dtype[numpy.float64]] = ..., + ) -> None: ... def add_postfix(self, id: str) -> None: ... class SimTilburgHandState(rcs._core.common.HandState): diff --git a/python/rcs/common_typing.py b/python/rcs/common_typing.py new file mode 100644 index 00000000..8d65cec0 --- /dev/null +++ b/python/rcs/common_typing.py @@ -0,0 +1,31 @@ +# ATTENTION: auto generated from C++ stub files, use `make stubgen` to update! +"""TypedDict helpers generated from `python/rcs/_core/common.pyi`.""" +from __future__ import annotations + +from typing import TypedDict + +import numpy +from rcs._core import common + +__all__ = ["BaseCameraConfigKwargs", "RobotConfigKwargs", "GripperConfigKwargs"] + + +class BaseCameraConfigKwargs(TypedDict, total=False): + identifier: str + frame_rate: int + resolution_width: int + resolution_height: int + + +class RobotConfigKwargs(TypedDict, total=False): + robot_type: common.RobotType + robot_platform: common.RobotPlatform + tcp_offset: common.Pose + attachment_site: str + kinematic_model_path: str + home_on_reset: bool + q_home: numpy.ndarray | None + + +class GripperConfigKwargs(TypedDict, total=False): + binary: bool diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index fb6cf410..d65f0ccb 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -284,13 +284,12 @@ class RobotWrapper(ActObsInfoWrapper): y """ - def __init__(self, env, robot: common.Robot, control_mode: ControlMode, home_on_reset: bool = False): + def __init__(self, env, robot: common.Robot, control_mode: ControlMode): super().__init__(env) self.robot = robot self._control_mode_overrides = [control_mode] self.action_space: gym.spaces.Dict self.observation_space: gym.spaces.Dict - self.home_on_reset = home_on_reset low, high = get_joint_limits(self.robot) if control_mode == ControlMode.JOINTS: self.action_space = get_space(JointsDictType, params={"joint_limits": {"low": low, "high": high}}) @@ -382,8 +381,12 @@ def reset( ) -> tuple[dict[str, Any], dict[str, Any]]: self.prev_action = None self.robot.reset() - if self.home_on_reset: - self.robot.move_home() + cfg = self.robot.get_config() + if cfg.home_on_reset: + if cfg.q_home is not None: + self.robot.set_joint_position(cfg.q_home) + else: + self.robot.move_home() return super().reset(seed=seed, options=options) def close(self): @@ -883,7 +886,7 @@ class GripperWrapper(ActObsInfoWrapper): BINARY_GRIPPER_CLOSED: ClassVar[list[float]] = [0] BINARY_GRIPPER_OPEN: ClassVar[list[float]] = [1] - def __init__(self, env, gripper: common.Gripper, binary: bool = True): + def __init__(self, env, gripper: common.Gripper): super().__init__(env) self.observation_space: gym.spaces.Dict self.observation_space.spaces.update(get_space(GripperDictType).spaces) @@ -891,7 +894,6 @@ def __init__(self, env, gripper: common.Gripper, binary: bool = True): self.action_space.spaces.update(get_space(GripperDictType).spaces) self.gripper_key = get_space_keys(GripperDictType)[0] self.gripper = gripper - self.binary = binary self._last_gripper_cmd = None def close(self): @@ -905,7 +907,7 @@ def reset(self, **kwargs) -> tuple[dict[str, Any], dict[str, Any]]: def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any]]: observation = copy.deepcopy(observation) - if self.binary: + if self.gripper.get_config().binary: observation[self.gripper_key] = ( self._last_gripper_cmd if self._last_gripper_cmd is not None else self.BINARY_GRIPPER_OPEN ) @@ -922,11 +924,11 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: gripper_action = action[self.gripper_key] if isinstance(gripper_action, int | float): gripper_action = [gripper_action] # type: ignore - if self.binary: + if self.gripper.get_config().binary: gripper_action = np.round(gripper_action) gripper_action = np.clip(gripper_action, 0.0, 1.0) - if self.binary: + if self.gripper.get_config().binary: self.gripper.grasp() if gripper_action == self.BINARY_GRIPPER_CLOSED else self.gripper.open() else: self.gripper.set_normalized_width(gripper_action[0]) diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index 5383b7fa..8dbce97a 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -98,7 +98,7 @@ def __call__( # type: ignore if gripper_cfg is not None and isinstance(gripper_cfg, rcs.sim.SimGripperConfig): gripper = sim.SimGripper(simulation, gripper_cfg) - env = GripperWrapper(env, gripper, binary=True) + env = GripperWrapper(env, gripper) else: gripper = None @@ -169,7 +169,7 @@ def __call__( # type: ignore gripper_cfg_copy = copy.copy(gripper_cfg) gripper_cfg_copy.add_postfix("_" + mid) gripper = rcs.sim.SimGripper(simulation, gripper_cfg_copy) - env = GripperWrapper(env, gripper, binary=True) + env = GripperWrapper(env, gripper) env = RobotSimWrapper(env) diff --git a/python/rcs/hand/tilburg_hand.py b/python/rcs/hand/tilburg_hand.py index 30b44a08..9800dc31 100644 --- a/python/rcs/hand/tilburg_hand.py +++ b/python/rcs/hand/tilburg_hand.py @@ -1,7 +1,6 @@ import copy import logging import typing -from dataclasses import dataclass from time import sleep import numpy as np @@ -15,27 +14,30 @@ logger.disabled = False -@dataclass(kw_only=True) class THConfig(common.HandConfig): """Config for the Tilburg hand""" - calibration_file: str | None = None - grasp_percentage: float = 1.0 - control_unit: Unit = Unit.NORMALIZED - hand_orientation: str = "right" - grasp_type: common.GraspType = common.GraspType.POWER_GRASP - - def __post_init__(self): - # 👇 satisfy pybind11 by actually calling the C++ constructor + def __init__( + self, + calibration_file: str | None = None, + grasp_percentage: float = 1.0, + control_unit: Unit = Unit.NORMALIZED, + hand_orientation: str = "right", + grasp_type: common.GraspType = common.GraspType.POWER_GRASP, + ) -> None: super().__init__() + self.calibration_file = calibration_file + self.grasp_percentage = grasp_percentage + self.control_unit = control_unit + self.hand_orientation = hand_orientation + self.grasp_type = grasp_type -@dataclass class TilburgHandState(common.HandState): - joint_positions: Vec18Type - def __post_init__(self): + def __init__(self, joint_positions: Vec18Type) -> None: super().__init__() + self.joint_positions = joint_positions class TilburgHand(common.Hand): diff --git a/scripts/generate_common_typing.py b/scripts/generate_common_typing.py new file mode 100644 index 00000000..2f81b88d --- /dev/null +++ b/scripts/generate_common_typing.py @@ -0,0 +1,251 @@ +from __future__ import annotations + +import argparse +import ast +from dataclasses import dataclass +from pathlib import Path + + +@dataclass(frozen=True) +class TypedDictSpec: + source_class: str + output_class: str + + @classmethod + def from_class_name(cls, class_name: str) -> "TypedDictSpec": + return cls(source_class=class_name, output_class=f"{class_name}Kwargs") + + +TARGETS = tuple( + TypedDictSpec.from_class_name(class_name) + for class_name in ( + "BaseCameraConfig", + "RobotConfig", + "GripperConfig", + ) +) + + +class _QualifyCommonNames(ast.NodeTransformer): + def __init__(self, common_class_names: set[str]) -> None: + self.common_class_names = common_class_names + + def visit_Name(self, node: ast.Name) -> ast.AST: + if node.id in self.common_class_names: + return ast.copy_location( + ast.Attribute(value=ast.Name(id="common", ctx=ast.Load()), attr=node.id, ctx=ast.Load()), + node, + ) + return node + + +class _SimplifyNdarrayAnnotations(ast.NodeTransformer): + def visit_Subscript(self, node: ast.Subscript) -> ast.AST: + node = self.generic_visit(node) + if isinstance(node.value, ast.Attribute) and isinstance(node.value.value, ast.Name): + if node.value.value.id == "numpy" and node.value.attr == "ndarray": + return ast.copy_location(node.value, node) + return node + + +class _ReferencedNames(ast.NodeVisitor): + def __init__(self) -> None: + self.names: set[str] = set() + + def visit_Name(self, node: ast.Name) -> None: + self.names.add(node.id) + + +def _names_in_node(node: ast.AST) -> set[str]: + referenced_names = _ReferencedNames() + referenced_names.visit(node) + return referenced_names.names + + +def _find_class(module: ast.Module, class_name: str) -> ast.ClassDef: + for node in module.body: + if isinstance(node, ast.ClassDef) and node.name == class_name: + return node + + msg = f"Could not find class {class_name!r} in stub file." + raise ValueError(msg) + + +def _find_init_signature(class_node: ast.ClassDef) -> ast.FunctionDef: + for node in class_node.body: + if isinstance(node, ast.FunctionDef) and node.name == "__init__": + return node + + msg = f"Could not find __init__ for class {class_node.name!r}." + raise ValueError(msg) + + +def _annotation_to_text( + annotation: ast.expr | None, + qualifier: _QualifyCommonNames, +) -> tuple[str, set[str]]: + if annotation is None: + return "object", set() + + qualified_annotation = qualifier.visit(ast.fix_missing_locations(annotation)) + qualified_annotation = _SimplifyNdarrayAnnotations().visit(ast.fix_missing_locations(qualified_annotation)) + referenced_names = _ReferencedNames() + referenced_names.visit(qualified_annotation) + return ast.unparse(qualified_annotation), referenced_names.names + + +def _render_typeddict( + spec: TypedDictSpec, + module: ast.Module, + qualifier: _QualifyCommonNames, +) -> tuple[list[str], set[str]]: + class_node = _find_class(module, spec.source_class) + init_node = _find_init_signature(class_node) + lines = [f"class {spec.output_class}(TypedDict, total=False):"] + referenced_names: set[str] = set() + + for arg in init_node.args.args[1:]: + annotation_text, annotation_names = _annotation_to_text(arg.annotation, qualifier) + referenced_names.update(annotation_names) + lines.append(f" {arg.arg}: {annotation_text}") + + if len(lines) == 1: + lines.append(" pass") + return lines, referenced_names + + +def _top_level_definitions_by_name(module: ast.Module, source: str) -> dict[str, str]: + definitions: dict[str, str] = {} + for node in module.body: + if isinstance(node, ast.Assign): + for target in node.targets: + if isinstance(target, ast.Name): + source_segment = ast.get_source_segment(source, node) + if source_segment is not None: + definitions[target.id] = source_segment + elif isinstance(node, ast.AnnAssign) and isinstance(node.target, ast.Name): + source_segment = ast.get_source_segment(source, node) + if source_segment is not None: + definitions[node.target.id] = source_segment + return definitions + + +def _top_level_definition_nodes_by_name(module: ast.Module) -> dict[str, ast.AST]: + definition_nodes: dict[str, ast.AST] = {} + for node in module.body: + if isinstance(node, ast.Assign): + for target in node.targets: + if isinstance(target, ast.Name): + definition_nodes[target.id] = node + elif isinstance(node, ast.AnnAssign) and isinstance(node.target, ast.Name): + definition_nodes[node.target.id] = node + return definition_nodes + + +def _imports_by_bound_name(module: ast.Module, source: str) -> dict[str, str]: + imports: dict[str, str] = {} + for node in module.body: + if not isinstance(node, ast.Import | ast.ImportFrom): + continue + + source_segment = ast.get_source_segment(source, node) + if source_segment is None: + continue + + for alias in node.names: + bound_name = alias.asname or alias.name.split(".")[0] + imports[bound_name] = source_segment + return imports + + +def generate_common_typing(stub_path: Path, output_path: Path) -> None: + source = stub_path.read_text() + module = ast.parse(source) + common_class_names = {node.name for node in module.body if isinstance(node, ast.ClassDef)} + qualifier = _QualifyCommonNames(common_class_names) + imports_by_name = _imports_by_bound_name(module, source) + definitions_by_name = _top_level_definitions_by_name(module, source) + definition_nodes_by_name = _top_level_definition_nodes_by_name(module) + + rendered_targets: list[str] = [] + referenced_names: set[str] = set() + output_names: list[str] = [] + + for spec in TARGETS: + lines, target_names = _render_typeddict(spec, module, qualifier) + rendered_targets.extend(lines) + rendered_targets.append("") + referenced_names.update(target_names) + output_names.append(spec.output_class) + + required_names = set(referenced_names - {"common"}) + pending_names = list(required_names) + import_lines: list[str] = [] + definition_lines: list[str] = [] + seen_import_lines: set[str] = set() + seen_definition_names: set[str] = set() + + while pending_names: + name = pending_names.pop() + if name in imports_by_name: + import_line = imports_by_name[name] + if import_line not in seen_import_lines: + seen_import_lines.add(import_line) + import_lines.append(import_line) + continue + + if name not in definitions_by_name or name in seen_definition_names: + continue + + seen_definition_names.add(name) + definition_lines.append(definitions_by_name[name]) + dependency_names = _names_in_node(definition_nodes_by_name[name]) - {name} + for dependency_name in dependency_names: + if dependency_name not in required_names: + required_names.add(dependency_name) + pending_names.append(dependency_name) + + output_parts = [ + "# ATTENTION: auto generated from C++ stub files, use `make stubgen` to update!", + f'"""TypedDict helpers generated from `{stub_path.as_posix()}`."""', + "from __future__ import annotations", + "", + "from typing import TypedDict", + "", + "from rcs._core import common", + ] + if import_lines: + output_parts.extend(["", *sorted(import_lines)]) + if definition_lines: + output_parts.extend(["", *definition_lines]) + output_parts.extend( + [ + "", + f"__all__ = {output_names!r}", + "", + *rendered_targets[:-1], + ] + ) + output_path.write_text("\n".join(output_parts) + "\n") + + +def main() -> None: + parser = argparse.ArgumentParser(description="Generate TypedDict helpers from common.pyi.") + parser.add_argument( + "--stub-path", + default="python/rcs/_core/common.pyi", + type=Path, + help="Path to the generated common.pyi stub file.", + ) + parser.add_argument( + "--output-path", + default="python/rcs/common_typing.py", + type=Path, + help="Path where the TypedDict helper module should be written.", + ) + args = parser.parse_args() + generate_common_typing(args.stub_path, args.output_path) + + +if __name__ == "__main__": + main() diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 0177580f..007ee475 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -234,16 +234,31 @@ PYBIND11_MODULE(_core, m) { common.def("FrankaHandTCPOffset", &rcs::common::FrankaHandTCPOffset); py::class_(common, "BaseCameraConfig") - .def(py::init(), py::arg("identifier"), - py::arg("frame_rate"), py::arg("resolution_width"), - py::arg("resolution_height")) + .def(py::init([](const std::string& identifier, int frame_rate, + int resolution_width, int resolution_height) { + auto cfg = new rcs::common::BaseCameraConfig(); + cfg->identifier = identifier; + cfg->frame_rate = frame_rate; + cfg->resolution_width = resolution_width; + cfg->resolution_height = resolution_height; + return cfg; + }), + py::arg("identifier"), py::arg("frame_rate"), + py::arg("resolution_width"), py::arg("resolution_height")) .def_readwrite("identifier", &rcs::common::BaseCameraConfig::identifier) .def_readwrite("frame_rate", &rcs::common::BaseCameraConfig::frame_rate) .def_readwrite("resolution_width", &rcs::common::BaseCameraConfig::resolution_width) .def_readwrite("resolution_height", - &rcs::common::BaseCameraConfig::resolution_height); - + &rcs::common::BaseCameraConfig::resolution_height) + .def("__copy__", + [](const rcs::common::BaseCameraConfig& self) { + return rcs::common::BaseCameraConfig(self); + }) + .def("__deepcopy__", + [](const rcs::common::BaseCameraConfig& self, py::dict) { + return rcs::common::BaseCameraConfig(self); + }); py::class_(common, "RPY") .def(py::init(), py::arg("roll") = 0.0, py::arg("pitch") = 0.0, py::arg("yaw") = 0.0) @@ -362,8 +377,33 @@ PYBIND11_MODULE(_core, m) { }, py::arg("robot_type")); + rcs::common::RobotConfig default_robot_config; py::class_(common, "RobotConfig") - .def(py::init<>()) + .def(py::init([](rcs::common::RobotType robot_type, + rcs::common::RobotPlatform robot_platform, + const rcs::common::Pose& tcp_offset, + const std::string& attachment_site, + const std::string& kinematic_model_path, + bool home_on_reset, + std::optional q_home) { + rcs::common::RobotConfig config; + config.robot_type = robot_type; + config.robot_platform = robot_platform; + config.tcp_offset = tcp_offset; + config.attachment_site = attachment_site; + config.kinematic_model_path = kinematic_model_path; + config.home_on_reset = home_on_reset; + config.q_home = q_home; + return config; + }), + py::arg("robot_type") = default_robot_config.robot_type, + py::arg("robot_platform") = default_robot_config.robot_platform, + py::arg("tcp_offset") = default_robot_config.tcp_offset, + py::arg("attachment_site") = default_robot_config.attachment_site, + py::arg("kinematic_model_path") = + default_robot_config.kinematic_model_path, + py::arg("home_on_reset") = default_robot_config.home_on_reset, + py::arg("q_home") = default_robot_config.q_home) .def_readwrite("robot_type", &rcs::common::RobotConfig::robot_type) .def_readwrite("kinematic_model_path", &rcs::common::RobotConfig::kinematic_model_path) @@ -371,10 +411,19 @@ PYBIND11_MODULE(_core, m) { &rcs::sim::SimRobotConfig::attachment_site) .def_readwrite("tcp_offset", &rcs::sim::SimRobotConfig::tcp_offset) .def_readwrite("robot_platform", - &rcs::common::RobotConfig::robot_platform); + &rcs::common::RobotConfig::robot_platform) + .def_readwrite("home_on_reset", &rcs::common::RobotConfig::home_on_reset) + .def_readwrite("q_home", &rcs::common::RobotConfig::q_home); py::class_(common, "RobotState").def(py::init<>()); + rcs::common::GripperConfig default_gripper_config; py::class_(common, "GripperConfig") - .def(py::init<>()); + .def(py::init([](bool binary) { + rcs::common::GripperConfig config; + config.binary = binary; + return config; + }), + py::arg("binary") = default_gripper_config.binary) + .def_readwrite("binary", &rcs::common::GripperConfig::binary); py::class_(common, "GripperState") .def(py::init<>()); py::enum_(common, "GraspType") @@ -458,9 +507,52 @@ PYBIND11_MODULE(_core, m) { // SIM MODULE auto sim = m.def_submodule("sim", "sim module"); + rcs::sim::SimRobotConfig default_simrobot_cfg = rcs::sim::SimRobotConfig(); py::class_( sim, "SimRobotConfig") - .def(py::init<>()) + .def( + py::init([](rcs::common::RobotType robot_type, + rcs::common::Pose tcp_offset, std::string attachment_site, + std::string kinematic_model_path, + double joint_rotational_tolerance, + double seconds_between_callbacks, + std::string mjcf_scene_path, bool trajectory_trace, + std::vector arm_collision_geoms, + std::vector joints, + std::vector actuators, std::string base) { + rcs::sim::SimRobotConfig config; + config.robot_type = robot_type; + config.robot_platform = rcs::common::RobotPlatform::SIMULATION; + config.tcp_offset = tcp_offset; + config.attachment_site = attachment_site; + config.kinematic_model_path = kinematic_model_path; + config.joint_rotational_tolerance = joint_rotational_tolerance; + config.seconds_between_callbacks = seconds_between_callbacks; + config.mjcf_scene_path = mjcf_scene_path; + config.trajectory_trace = trajectory_trace; + config.arm_collision_geoms = arm_collision_geoms; + config.joints = joints; + config.actuators = actuators; + config.base = base; + return config; + }), + py::arg("robot_type") = default_simrobot_cfg.robot_type, + py::arg("tcp_offset") = default_simrobot_cfg.tcp_offset, + py::arg("attachment_site") = default_simrobot_cfg.attachment_site, + py::arg("kinematic_model_path") = + default_simrobot_cfg.kinematic_model_path, + py::arg("joint_rotational_tolerance") = + default_simrobot_cfg.joint_rotational_tolerance, + py::arg("seconds_between_callbacks") = + default_simrobot_cfg.seconds_between_callbacks, + py::arg("mjcf_scene_path") = default_simrobot_cfg.mjcf_scene_path, + py::arg("trajectory_trace") = default_simrobot_cfg.trajectory_trace, + py::arg("arm_collision_geoms") = + default_simrobot_cfg.arm_collision_geoms, + py::arg("joints") = default_simrobot_cfg.joints, + py::arg("actuators") = default_simrobot_cfg.actuators, + py::arg("base") = default_simrobot_cfg.base) + .def_readwrite("joint_rotational_tolerance", &rcs::sim::SimRobotConfig::joint_rotational_tolerance) .def_readwrite("seconds_between_callbacks", @@ -496,9 +588,51 @@ PYBIND11_MODULE(_core, m) { .def_readonly("collision", &rcs::sim::SimRobotState::collision) .def_readonly("is_moving", &rcs::sim::SimRobotState::is_moving) .def_readonly("is_arrived", &rcs::sim::SimRobotState::is_arrived); + + rcs::sim::SimGripperConfig default_simgripper_cfg = + rcs::sim::SimGripperConfig(); py::class_( sim, "SimGripperConfig") - .def(py::init<>()) + .def(py::init([](double epsilon_inner, double epsilon_outer, + double seconds_between_callbacks, + std::vector ignored_collision_geoms, + std::vector collision_geoms, + std::vector collision_geoms_fingers, + std::vector joints, double max_joint_width, + double min_joint_width, std::string actuator, + double max_actuator_width, double min_actuator_width) { + rcs::sim::SimGripperConfig config; + config.epsilon_inner = epsilon_inner; + config.epsilon_outer = epsilon_outer; + config.seconds_between_callbacks = seconds_between_callbacks; + config.ignored_collision_geoms = ignored_collision_geoms; + config.collision_geoms = collision_geoms; + config.collision_geoms_fingers = collision_geoms_fingers; + config.joints = joints; + config.max_joint_width = max_joint_width; + config.min_joint_width = min_joint_width; + config.actuator = actuator; + config.max_actuator_width = max_actuator_width; + config.min_actuator_width = min_actuator_width; + return config; + }), + py::arg("epsilon_inner") = default_simgripper_cfg.epsilon_inner, + py::arg("epsilon_outer") = default_simgripper_cfg.epsilon_outer, + py::arg("seconds_between_callbacks") = + default_simgripper_cfg.seconds_between_callbacks, + py::arg("ignored_collision_geoms") = + default_simgripper_cfg.ignored_collision_geoms, + py::arg("collision_geoms") = default_simgripper_cfg.collision_geoms, + py::arg("collision_geoms_fingers") = + default_simgripper_cfg.collision_geoms_fingers, + py::arg("joints") = default_simgripper_cfg.joints, + py::arg("max_joint_width") = default_simgripper_cfg.max_joint_width, + py::arg("min_joint_width") = default_simgripper_cfg.min_joint_width, + py::arg("actuator") = default_simgripper_cfg.actuator, + py::arg("max_actuator_width") = + default_simgripper_cfg.max_actuator_width, + py::arg("min_actuator_width") = + default_simgripper_cfg.min_actuator_width) .def_readwrite("epsilon_inner", &rcs::sim::SimGripperConfig::epsilon_inner) .def_readwrite("epsilon_outer", @@ -540,13 +674,34 @@ PYBIND11_MODULE(_core, m) { .def_readonly("last_width", &rcs::sim::SimGripperState::last_width) .def_readonly("collision", &rcs::sim::SimGripperState::collision); + rcs::sim::SimConfig default_sim_cfg = rcs::sim::SimConfig(); py::class_(sim, "SimConfig") - .def(py::init<>()) + .def(py::init([](bool async_control, bool realtime, double frequency, + int max_convergence_steps) { + rcs::sim::SimConfig config; + config.async_control = async_control; + config.realtime = realtime; + config.frequency = frequency; + config.max_convergence_steps = max_convergence_steps; + return config; + }), + py::arg("async_control") = default_sim_cfg.async_control, + py::arg("realtime") = default_sim_cfg.realtime, + py::arg("frequency") = default_sim_cfg.frequency, + py::arg("max_convergence_steps") = + default_sim_cfg.max_convergence_steps) .def_readwrite("async_control", &rcs::sim::SimConfig::async_control) .def_readwrite("realtime", &rcs::sim::SimConfig::realtime) .def_readwrite("frequency", &rcs::sim::SimConfig::frequency) .def_readwrite("max_convergence_steps", - &rcs::sim::SimConfig::max_convergence_steps); + &rcs::sim::SimConfig::max_convergence_steps) + .def("__copy__", + [](const rcs::sim::SimConfig& self) { + return rcs::sim::SimConfig(self); + }) + .def("__deepcopy__", [](const rcs::sim::SimConfig& self, py::dict) { + return rcs::sim::SimConfig(self); + }); py::class_>(sim, "Sim") .def(py::init([](long m, long d) { @@ -596,9 +751,46 @@ PYBIND11_MODULE(_core, m) { .def_readonly("last_qpos", &rcs::sim::SimTilburgHandState::last_qpos) .def_readonly("collision", &rcs::sim::SimTilburgHandState::collision); // SimTilburgHandConfig + rcs::sim::SimTilburgHandConfig default_simtilburghand_cfg = + rcs::sim::SimTilburgHandConfig(); py::class_( sim, "SimTilburgHandConfig") - .def(py::init<>()) + .def(py::init([](rcs::common::GraspType grasp_type, + double seconds_between_callbacks, + std::vector ignored_collision_geoms, + std::vector collision_geoms, + std::vector collision_geoms_fingers, + std::vector joints, + std::vector actuators, + rcs::common::Vector16d max_joint_position, + rcs::common::Vector16d min_joint_position) { + rcs::sim::SimTilburgHandConfig config; + config.grasp_type = grasp_type; + config.seconds_between_callbacks = seconds_between_callbacks; + config.ignored_collision_geoms = ignored_collision_geoms; + config.collision_geoms = collision_geoms; + config.collision_geoms_fingers = collision_geoms_fingers; + config.joints = joints; + config.actuators = actuators; + config.max_joint_position = max_joint_position; + config.min_joint_position = min_joint_position; + return config; + }), + py::arg("grasp_type") = default_simtilburghand_cfg.grasp_type, + py::arg("seconds_between_callbacks") = + default_simtilburghand_cfg.seconds_between_callbacks, + py::arg("ignored_collision_geoms") = + default_simtilburghand_cfg.ignored_collision_geoms, + py::arg("collision_geoms") = + default_simtilburghand_cfg.collision_geoms, + py::arg("collision_geoms_fingers") = + default_simtilburghand_cfg.collision_geoms_fingers, + py::arg("joints") = default_simtilburghand_cfg.joints, + py::arg("actuators") = default_simtilburghand_cfg.actuators, + py::arg("max_joint_position") = + default_simtilburghand_cfg.max_joint_position, + py::arg("min_joint_position") = + default_simtilburghand_cfg.min_joint_position) .def_readwrite("max_joint_position", &rcs::sim::SimTilburgHandConfig::max_joint_position) .def_readwrite("min_joint_position", @@ -615,7 +807,15 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("seconds_between_callbacks", &rcs::sim::SimTilburgHandConfig::seconds_between_callbacks) .def("add_postfix", &rcs::sim::SimTilburgHandConfig::add_postfix, - py::arg("id")); + py::arg("id")) + .def("__copy__", + [](const rcs::sim::SimTilburgHandConfig& self) { + return rcs::sim::SimTilburgHandConfig(self); + }) + .def("__deepcopy__", + [](const rcs::sim::SimTilburgHandConfig& self, py::dict) { + return rcs::sim::SimTilburgHandConfig(self); + }); // SimTilburgHand py::class_>(sim, "SimTilburgHand") @@ -634,13 +834,36 @@ PYBIND11_MODULE(_core, m) { .export_values(); py::class_( sim, "SimCameraConfig") - .def(py::init(), + .def(py::init([](const std::string& identifier, int frame_rate, + int resolution_width, int resolution_height, + rcs::sim::CameraType type) { + rcs::sim::SimCameraConfig config; + config.identifier = identifier; + config.frame_rate = frame_rate; + config.resolution_width = resolution_width; + config.resolution_height = resolution_height; + config.type = type; + return config; + }), py::arg("identifier"), py::arg("frame_rate"), py::arg("resolution_width"), py::arg("resolution_height"), py::arg("type") = rcs::sim::CameraType::fixed) .def_readwrite("type", &rcs::sim::SimCameraConfig::type); py::class_(sim, "FrameSet") - .def(py::init<>()) + .def(py::init( + [](const std::unordered_map& + color_frames, + const std::unordered_map& + depth_frames, + double timestamp) { + rcs::sim::FrameSet fs; + fs.color_frames = color_frames; + fs.depth_frames = depth_frames; + fs.timestamp = timestamp; + return fs; + }), + py::arg("color_frames"), py::arg("depth_frames"), + py::arg("timestamp")) .def_readonly("color_frames", &rcs::sim::FrameSet::color_frames) .def_readonly("depth_frames", &rcs::sim::FrameSet::depth_frames) .def_readonly("timestamp", &rcs::sim::FrameSet::timestamp); diff --git a/src/sim/SimRobot.h b/src/sim/SimRobot.h index 0dfac8c0..d8983d93 100644 --- a/src/sim/SimRobot.h +++ b/src/sim/SimRobot.h @@ -12,6 +12,7 @@ namespace rcs { namespace sim { struct SimRobotConfig : common::RobotConfig { + common::RobotPlatform robot_platform = common::RobotPlatform::SIMULATION; double joint_rotational_tolerance = .05 * (std::numbers::pi / 180.0); // 0.05 degree double seconds_between_callbacks = 0.1; // 10 Hz diff --git a/src/sim/camera.h b/src/sim/camera.h index 8b3daa25..180ae7ad 100644 --- a/src/sim/camera.h +++ b/src/sim/camera.h @@ -25,12 +25,6 @@ enum CameraType { struct SimCameraConfig : common::BaseCameraConfig { CameraType type; - SimCameraConfig(const std::string& identifier, int frame_rate, - int resolution_width, int resolution_height, - CameraType type = fixed) - : common::BaseCameraConfig(identifier, frame_rate, resolution_width, - resolution_height), - type(type) {} }; // (H,W,3)