diff --git a/README.md b/README.md index bc5558a4..e391e2fc 100644 --- a/README.md +++ b/README.md @@ -28,7 +28,7 @@ $ conda env create -f rpc.yml - [Foxglove](https://github.com/foxglove): websocket & schema protocols for robot visualization and parameter operations -## :computer: Usage +## :computer: Usage #### (1) PyBullet - Source conda environment:
``` @@ -56,7 +56,7 @@ $ make -j4 $ ./bin/run_draco ``` #### Keyboard Input -- Please see the [example implementation](https://github.com/shbang91/rpc/blob/develop/controller/draco_controller/draco_interrupt_handler.cpp) for DRACO 3 +- Please see the [example implementation](https://github.com/shbang91/rpc/blob/develop/controller/draco_controller/draco_interrupt_handler.cpp) for DRACO 3 ## :tv: Visualization #### (1) Foxglove UI (optional) diff --git a/binding/pycontroller/optimo_controller/CMakeLists.txt b/binding/pycontroller/optimo_controller/CMakeLists.txt index 0dc1ad8b..92c8f439 100644 --- a/binding/pycontroller/optimo_controller/CMakeLists.txt +++ b/binding/pycontroller/optimo_controller/CMakeLists.txt @@ -1,2 +1,2 @@ pybind11_add_module(optimo_interface_py optimo_interface_pybind_pin.cpp) -target_link_libraries(optimo_interface_py PUBLIC rpc-optimo-controller) \ No newline at end of file +target_link_libraries(optimo_interface_py PUBLIC rpc-optimo-controller) diff --git a/config/go2/sim/pybullet/wbic/pnc.yaml b/config/go2/sim/pybullet/wbic/pnc.yaml index aee01879..f6443160 100644 --- a/config/go2/sim/pybullet/wbic/pnc.yaml +++ b/config/go2/sim/pybullet/wbic/pnc.yaml @@ -8,7 +8,7 @@ stance_foot: 1 # 0: left_foot 1: right_foot state_estimator: b_cheater_mode: true ## true: use simulation groundtruth states - state_estimator_type: kf ## 1. default (IMU + Leg kinematics) 2. kf + state_estimator_type: kf ## 1. default (IMU + Leg kinematics) 2. kf b_use_marg_filter: false # only used when kf is set to true foot_reference_frame: 1 #0: left foot, 1: right foot @@ -34,7 +34,7 @@ state_estimator: num_data_com_vel: [10, 10, 10] ## IF 1: exponential smoother filter parameter - #com_vel_time_constant: 0.1 + #com_vel_time_constant: 0.1 com_vel_time_constant: 0.01 com_vel_err_limit: [1., 1., 1.] @@ -43,7 +43,7 @@ state_estimator: controller: - b_smoothing_command: true + b_smoothing_command: true smoothing_command_duration: 0.01 ## this will filter the control command at the beginning b_use_modified_swing_foot_jac: true b_use_filtered_torque: false @@ -102,7 +102,7 @@ wbc: mu: 0.5 foot_half_length: 0.08 foot_half_width: 0.04 ## 0.04 - kp: [0, 0, 0, 0, 0, 0] ## ori, pos order + kp: [0, 0, 0, 0, 0, 0] ## ori, pos order kd: [0, 0, 0, 0, 0, 0] qp: diff --git a/config/go2/sim/pybullet/wbic/pybullet_params.py b/config/go2/sim/pybullet/wbic/pybullet_params.py index d2fa1003..95ac84f8 100644 --- a/config/go2/sim/pybullet/wbic/pybullet_params.py +++ b/config/go2/sim/pybullet/wbic/pybullet_params.py @@ -71,12 +71,65 @@ class Go2JointIdx(object): class JointGains(object): # kp = 5. * np.ones(27) # kd = 0. * np.ones(27) - kp = np.array([ - 10, 10, 10, 10, 10, 10, 10, 5, 5, 5, 5, 5, 5, 5, 10, 10, 10, 10, 10, - 10, 10, 5, 5, 5, 5, 5, 5 - ]) - kd = np.array([ - 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.001, 0.001, 0.001, 0.001, - 0.001, 0.001, 0.01, 0.01, 0.001, 0.01, 0.01, 0.01, 0.01, 0.01, 0.001, - 0.001, 0.001, 0.001, 0.001, 0.001 - ]) + kp = np.array( + [ + 10, + 10, + 10, + 10, + 10, + 10, + 10, + 5, + 5, + 5, + 5, + 5, + 5, + 5, + 10, + 10, + 10, + 10, + 10, + 10, + 10, + 5, + 5, + 5, + 5, + 5, + 5, + ] + ) + kd = np.array( + [ + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.001, + 0.001, + 0.001, + 0.001, + 0.001, + 0.001, + 0.01, + 0.01, + 0.001, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.001, + 0.001, + 0.001, + 0.001, + 0.001, + 0.001, + ] + ) diff --git a/config/optimo/ihwbc_gains.yaml b/config/optimo/ihwbc_gains.yaml index f80cf462..dd91f84c 100644 --- a/config/optimo/ihwbc_gains.yaml +++ b/config/optimo/ihwbc_gains.yaml @@ -77,7 +77,7 @@ wbc: exp_weight_at_contact : [0., 0., 0.] exp_kp : [200., 200., 200.] exp_kd : [10., 10., 10.] - + f3_pos_task: ######################################################################## ## sim params @@ -182,19 +182,19 @@ wbc: ## sim params ######################################################################## sim_weight : [0., 0., 0., 1e-8, 1e-8, 5e-10] ## trq, force order - + f1_rf_task: ######################################################################## ## sim params ######################################################################## - sim_weight : [0., 0., 0., 1e-8, 1e-8, 5e-10] ## trq, force order - + sim_weight : [0., 0., 0., 1e-8, 1e-8, 5e-10] ## trq, force order + f2_rf_task: ######################################################################## ## sim params ######################################################################## - sim_weight : [0., 0., 0., 1e-8, 1e-8, 5e-10] ## trq, force order - + sim_weight : [0., 0., 0., 1e-8, 1e-8, 5e-10] ## trq, force order + f3_rf_task: ######################################################################## ## sim params @@ -241,7 +241,7 @@ wbc: state_machine: initialize: init_duration: 2. - wait_time: 0. + wait_time: 0. target_joint_pos: [0.0, 3.665191429188092, 0.0, -2.0943951023931953, 0.0, -1.0471975511965976, 0.0] b_stay_here: false task_transit_time: 0.3 @@ -250,7 +250,7 @@ state_machine: duration: 0.01 wait_time: 0. b_stay_here: false - + ee_traj: duration: 1. wait_time: 0. diff --git a/config/optimo/pybullet_simulation.py b/config/optimo/pybullet_simulation.py index 33d1b7ea..24b2fd6a 100644 --- a/config/optimo/pybullet_simulation.py +++ b/config/optimo/pybullet_simulation.py @@ -1,4 +1,3 @@ - def deg2rad(deg): return deg * 3.14159265359 / 180 @@ -10,9 +9,12 @@ class Config(object): INITIAL_BASE_JOINT_QUAT = [0, 0, 0, 1] INITIAL_JOINT_POSITION = [ deg2rad(0), - deg2rad(180), 0.0, - deg2rad(-90), 0.0, - deg2rad(-90), 0.0 + deg2rad(180), + 0.0, + deg2rad(-90), + 0.0, + deg2rad(-90), + 0.0, ] PRINT_ROBOT_INFO = True @@ -71,5 +73,5 @@ class PlatoJointIdx(object): class ActuatorGains(object): # KP = [200., 200., 200., 200., 200., 200., 200.] # KD = [5., 5., 5., 5., 5., 5., 5.] - KP = [0., 0., 0., 0., 0., 0., 0.] - KD = [0., 0., 0., 0., 0., 0., 0.] + KP = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + KD = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] diff --git a/config/optimo/sim/mujoco/ihwbc/pnc.yaml b/config/optimo/sim/mujoco/ihwbc/pnc.yaml index 2a37f4fc..e884c1f7 100644 --- a/config/optimo/sim/mujoco/ihwbc/pnc.yaml +++ b/config/optimo/sim/mujoco/ihwbc/pnc.yaml @@ -59,7 +59,7 @@ wbc: state_machine: initialize: duration: 2. - wait_time: 0. + wait_time: 0. target_joint_pos: [0.0, 3.665191429188092, 0.0, -2.0943951023931953, 0.0, -1.0471975511965976, 0.0] b_stay_here: false diff --git a/config/optimo/sim/pybullet/ihwbc/pnc.yaml b/config/optimo/sim/pybullet/ihwbc/pnc.yaml index 5bc4a8bf..243cc706 100644 --- a/config/optimo/sim/pybullet/ihwbc/pnc.yaml +++ b/config/optimo/sim/pybullet/ihwbc/pnc.yaml @@ -59,7 +59,7 @@ wbc: state_machine: initialize: duration: 2. - wait_time: 0. + wait_time: 0. target_joint_pos: [0.0, 3.665191429188092, 0.0, -2.0943951023931953, 0.0, -1.0471975511965976, 0.0] b_stay_here: false diff --git a/config/optimo/sim/pybullet/ihwbc/pybullet_params.py b/config/optimo/sim/pybullet/ihwbc/pybullet_params.py index 0bda7adf..24b2fd6a 100644 --- a/config/optimo/sim/pybullet/ihwbc/pybullet_params.py +++ b/config/optimo/sim/pybullet/ihwbc/pybullet_params.py @@ -9,9 +9,12 @@ class Config(object): INITIAL_BASE_JOINT_QUAT = [0, 0, 0, 1] INITIAL_JOINT_POSITION = [ deg2rad(0), - deg2rad(180), 0.0, - deg2rad(-90), 0.0, - deg2rad(-90), 0.0 + deg2rad(180), + 0.0, + deg2rad(-90), + 0.0, + deg2rad(-90), + 0.0, ] PRINT_ROBOT_INFO = True @@ -70,5 +73,5 @@ class PlatoJointIdx(object): class ActuatorGains(object): # KP = [200., 200., 200., 200., 200., 200., 200.] # KD = [5., 5., 5., 5., 5., 5., 5.] - KP = [0., 0., 0., 0., 0., 0., 0.] - KD = [0., 0., 0., 0., 0., 0., 0.] + KP = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + KD = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] diff --git a/controller/control_architecture.hpp b/controller/control_architecture.hpp index 2a0aece0..1aec0953 100644 --- a/controller/control_architecture.hpp +++ b/controller/control_architecture.hpp @@ -8,7 +8,7 @@ class ControlArchitecture { public: ControlArchitecture(PinocchioRobotSystem *robot) : robot_(robot), b_loco_state_first_visit_(true), - b_manip_state_first_visit_(true){}; + b_manip_state_first_visit_(true) {}; virtual ~ControlArchitecture() = default; virtual void GetCommand(void *command) = 0; diff --git a/controller/draco_controller/draco_interface.hpp b/controller/draco_controller/draco_interface.hpp index 61d8de88..9756adb0 100644 --- a/controller/draco_controller/draco_interface.hpp +++ b/controller/draco_controller/draco_interface.hpp @@ -21,7 +21,7 @@ class DracoSensorData { b_rf_contact_(false), lf_contact_normal_(0.), rf_contact_normal_(0.), base_joint_pos_(Eigen::Vector3d::Zero()), base_joint_quat_(0, 0, 0, 1), base_joint_lin_vel_(Eigen::Vector3d::Zero()), - base_joint_ang_vel_(Eigen::Vector3d::Zero()){}; + base_joint_ang_vel_(Eigen::Vector3d::Zero()) {}; virtual ~DracoSensorData() = default; Eigen::Vector4d imu_frame_quat_; // x, y, z, w order @@ -47,7 +47,7 @@ class DracoCommand { DracoCommand() : joint_pos_cmd_(Eigen::VectorXd::Zero(draco::n_adof)), joint_vel_cmd_(Eigen::VectorXd::Zero(draco::n_adof)), - joint_trq_cmd_(Eigen::VectorXd::Zero(draco::n_adof)){}; + joint_trq_cmd_(Eigen::VectorXd::Zero(draco::n_adof)) {}; virtual ~DracoCommand() = default; Eigen::VectorXd joint_pos_cmd_; diff --git a/controller/go2_controller/go2_interface.hpp b/controller/go2_controller/go2_interface.hpp index d8bf2395..37d3195a 100644 --- a/controller/go2_controller/go2_interface.hpp +++ b/controller/go2_controller/go2_interface.hpp @@ -22,7 +22,7 @@ class Go2SensorData { RR_normal_force_(0.), base_joint_pos_(Eigen::Vector3d::Zero()), base_joint_quat_(0, 0, 0, 1), base_joint_lin_vel_(Eigen::Vector3d::Zero()), - base_joint_ang_vel_(Eigen::Vector3d::Zero()){}; + base_joint_ang_vel_(Eigen::Vector3d::Zero()) {}; virtual ~Go2SensorData() = default; Eigen::Vector4d imu_frame_quat_; // x, y, z, w order @@ -52,7 +52,7 @@ class Go2Command { Go2Command() : joint_pos_cmd_(Eigen::VectorXd::Zero(go2::n_adof)), joint_vel_cmd_(Eigen::VectorXd::Zero(go2::n_adof)), - joint_trq_cmd_(Eigen::VectorXd::Zero(go2::n_adof)){}; + joint_trq_cmd_(Eigen::VectorXd::Zero(go2::n_adof)) {}; virtual ~Go2Command() = default; Eigen::VectorXd joint_pos_cmd_; diff --git a/controller/interface.hpp b/controller/interface.hpp index 0edcc1f5..1f176751 100644 --- a/controller/interface.hpp +++ b/controller/interface.hpp @@ -7,7 +7,7 @@ class InterruptHandler; class Interface { public: - Interface() : count_(0), running_time_(0.){}; + Interface() : count_(0), running_time_(0.) {}; virtual ~Interface() = default; diff --git a/controller/optimo_controller/optimo_controller.cpp b/controller/optimo_controller/optimo_controller.cpp index 82d09891..64fda222 100644 --- a/controller/optimo_controller/optimo_controller.cpp +++ b/controller/optimo_controller/optimo_controller.cpp @@ -17,7 +17,7 @@ // TODO: #if B_USE_ZMQ -//#include "controller/optimo_controller/optimo_data_manager.hpp" +// #include "controller/optimo_controller/optimo_data_manager.hpp" #endif OptimoController::OptimoController(OptimoTCIContainer *tci_container, diff --git a/controller/optimo_controller/optimo_interface.cpp b/controller/optimo_controller/optimo_interface.cpp index 57f3aeb2..31ed6470 100644 --- a/controller/optimo_controller/optimo_interface.cpp +++ b/controller/optimo_controller/optimo_interface.cpp @@ -13,7 +13,7 @@ // TODO: #if B_USE_ZMQ -//#include "controller/optimo_controller/optimo_data_manager.hpp" +// #include "controller/optimo_controller/optimo_data_manager.hpp" #endif OptimoInterface::OptimoInterface() : Interface() { diff --git a/controller/optimo_controller/optimo_interface.hpp b/controller/optimo_controller/optimo_interface.hpp index 376e14c9..387a1dfc 100644 --- a/controller/optimo_controller/optimo_interface.hpp +++ b/controller/optimo_controller/optimo_interface.hpp @@ -18,7 +18,7 @@ class OptimoSensorData { joint_trq_(Eigen::VectorXd::Zero(optimo::n_adof)), joint_sea_trq_(Eigen::VectorXd::Zero(optimo::n_adof)), base_pos_(Eigen::Vector3d::Zero()), - base_quat_(Eigen::Vector4d(0., 0., 0., 1.)){}; + base_quat_(Eigen::Vector4d(0., 0., 0., 1.)) {}; virtual ~OptimoSensorData() = default; @@ -36,7 +36,7 @@ class OptimoCommand { OptimoCommand() : joint_pos_cmd_(Eigen::VectorXd::Zero(optimo::n_adof)), joint_vel_cmd_(Eigen::VectorXd::Zero(optimo::n_adof)), - joint_trq_cmd_(Eigen::VectorXd::Zero(optimo::n_adof)){}; + joint_trq_cmd_(Eigen::VectorXd::Zero(optimo::n_adof)) {}; virtual ~OptimoCommand() = default; Eigen::VectorXd joint_pos_cmd_; diff --git a/controller/optimo_controller/optimo_state_machines/initialize.hpp b/controller/optimo_controller/optimo_state_machines/initialize.hpp index 35443a10..3f65cec7 100644 --- a/controller/optimo_controller/optimo_state_machines/initialize.hpp +++ b/controller/optimo_controller/optimo_state_machines/initialize.hpp @@ -35,4 +35,4 @@ class Initialize : public StateMachine { double task_transit_time_; MinJerkCurveVec *min_jerk_curves_; -}; \ No newline at end of file +}; diff --git a/controller/optimo_controller/optimo_state_machines/stand_up.cpp b/controller/optimo_controller/optimo_state_machines/stand_up.cpp index fc777390..d6d80360 100644 --- a/controller/optimo_controller/optimo_state_machines/stand_up.cpp +++ b/controller/optimo_controller/optimo_state_machines/stand_up.cpp @@ -6,24 +6,23 @@ #include "controller/whole_body_controller/basic_task.hpp" #include "util/interpolation.hpp" -StandUp::StandUp(const StateId state_id, PinocchioRobotSystem *robot, - OptimoControlArchitecture *ctrl_arch) - : StateMachine(state_id, robot), ctrl_arch_(ctrl_arch), b_stay_here_(false), - wait_time_(0.), min_jerk_curves_(nullptr) { - util::PrettyConstructor(2, "StandUp"); +StandUp::StandUp(const StateId state_id, PinocchioRobotSystem *robot, + OptimoControlArchitecture *ctrl_arch) + : StateMachine(state_id, robot), ctrl_arch_(ctrl_arch), b_stay_here_(false), + wait_time_(0.), min_jerk_curves_(nullptr) { + util::PrettyConstructor(2, "StandUp"); - sp_ = OptimoStateProvider::GetStateProvider(); - target_joint_pos_ = Eigen::VectorXd::Zero(robot_->NumActiveDof()); - init_joint_pos_ = Eigen::VectorXd::Zero(robot_->NumActiveDof()); + sp_ = OptimoStateProvider::GetStateProvider(); + target_joint_pos_ = Eigen::VectorXd::Zero(robot_->NumActiveDof()); + init_joint_pos_ = Eigen::VectorXd::Zero(robot_->NumActiveDof()); } StandUp::~StandUp() { - if (min_jerk_curves_ != nullptr) { - delete min_jerk_curves_; - } + if (min_jerk_curves_ != nullptr) { + delete min_jerk_curves_; + } } - void StandUp::FirstVisit() { std::cout << "optimo_states::kStandUp" << std::endl; state_machine_start_time_ = sp_->current_time_; @@ -47,8 +46,7 @@ void StandUp::OneStep() { Eigen::VectorXd::Zero(target_joint_pos_.size()); if (min_jerk_curves_ == nullptr) - throw std::runtime_error( - "StandUp MinJerkCurve in StandUp StateMachine"); + throw std::runtime_error("StandUp MinJerkCurve in StandUp StateMachine"); for (unsigned int i(0); i < target_joint_pos_.size(); ++i) { des_joint_pos = min_jerk_curves_->Evaluate(state_machine_time_); @@ -71,9 +69,7 @@ bool StandUp::EndOfState() { } } -StateId StandUp::GetNextState() { - return 0; -} +StateId StandUp::GetNextState() { return 0; } void StandUp::SetParameters(const YAML::Node &node) { try { @@ -88,4 +84,4 @@ void StandUp::SetParameters(const YAML::Node &node) { << __FILE__ << "]" << std::endl; std::exit(EXIT_FAILURE); } -} \ No newline at end of file +} diff --git a/controller/optimo_controller/optimo_state_machines/stand_up.hpp b/controller/optimo_controller/optimo_state_machines/stand_up.hpp index 740f05e3..eeff28e8 100644 --- a/controller/optimo_controller/optimo_state_machines/stand_up.hpp +++ b/controller/optimo_controller/optimo_state_machines/stand_up.hpp @@ -8,7 +8,7 @@ class MinJerkCurveVec; class StandUp : public StateMachine { public: StandUp(const StateId state_id, PinocchioRobotSystem *robot, - OptimoControlArchitecture *ctrl_arch); + OptimoControlArchitecture *ctrl_arch); ~StandUp(); void FirstVisit() override; @@ -31,4 +31,4 @@ class StandUp : public StateMachine { double wait_time_; MinJerkCurveVec *min_jerk_curves_; -}; \ No newline at end of file +}; diff --git a/controller/optimo_controller/optimo_state_provider.cpp b/controller/optimo_controller/optimo_state_provider.cpp index da50f916..86a58662 100644 --- a/controller/optimo_controller/optimo_state_provider.cpp +++ b/controller/optimo_controller/optimo_state_provider.cpp @@ -9,24 +9,24 @@ OptimoStateProvider *OptimoStateProvider::GetStateProvider() { } OptimoStateProvider::OptimoStateProvider() { - util::PrettyConstructor(1, "OptimoStateProvider"); + util::PrettyConstructor(1, "OptimoStateProvider"); - servo_dt_ = 0.001; - data_save_freq_ = 1; + servo_dt_ = 0.001; + data_save_freq_ = 1; - count_ = 0; - current_time_ = 0.; + count_ = 0; + current_time_ = 0.; - b_f1_contact_ = false; - b_f2_contact_ = false; - b_f3_contact_ = false; + b_f1_contact_ = false; + b_f2_contact_ = false; + b_f3_contact_ = false; - state_ = optimo_states::kInitialize; - prev_state_ = optimo_states::kInitialize; + state_ = optimo_states::kInitialize; + prev_state_ = optimo_states::kInitialize; - des_ee_iso_ = Eigen::Isometry3d::Identity(); + des_ee_iso_ = Eigen::Isometry3d::Identity(); - planning_id_ = 0; + planning_id_ = 0; - rot_world_local_ = Eigen::Matrix3d::Identity(); + rot_world_local_ = Eigen::Matrix3d::Identity(); } diff --git a/controller/optimo_controller/optimo_task_gain_handler.cpp b/controller/optimo_controller/optimo_task_gain_handler.cpp index c1a14c2a..cbd2de6d 100644 --- a/controller/optimo_controller/optimo_task_gain_handler.cpp +++ b/controller/optimo_controller/optimo_task_gain_handler.cpp @@ -6,7 +6,8 @@ #include "controller/whole_body_controller/task.hpp" #include "util/util.hpp" -OptimoTaskGainHandler::OptimoTaskGainHandler(OptimoControlArchitecture *ctrl_arch) +OptimoTaskGainHandler::OptimoTaskGainHandler( + OptimoControlArchitecture *ctrl_arch) : ctrl_arch_(ctrl_arch), b_signal_received_(false), b_first_visit_(false), b_com_xy_task_(false), count_(0) { @@ -18,9 +19,9 @@ OptimoTaskGainHandler::OptimoTaskGainHandler(OptimoControlArchitecture *ctrl_arc } void OptimoTaskGainHandler::Trigger(const std::string &task_name, - const Eigen::VectorXd &weight, - const Eigen::VectorXd &kp, - const Eigen::VectorXd &kd) { + const Eigen::VectorXd &weight, + const Eigen::VectorXd &kp, + const Eigen::VectorXd &kd) { task_name_ = task_name; if (!_TaskExists(task_name_)) return; @@ -33,10 +34,10 @@ void OptimoTaskGainHandler::Trigger(const std::string &task_name, } void OptimoTaskGainHandler::Trigger(const std::string &task_name, - const Eigen::VectorXd &weight, - const Eigen::VectorXd &kp, - const Eigen::VectorXd &kd, - const Eigen::VectorXd &ki) { + const Eigen::VectorXd &weight, + const Eigen::VectorXd &kp, + const Eigen::VectorXd &kd, + const Eigen::VectorXd &ki) { task_name_ = task_name; if (!_TaskExists(task_name_)) return; @@ -122,4 +123,4 @@ bool OptimoTaskGainHandler::_TaskExists(const std::string &task_name) { } return true; -} \ No newline at end of file +} diff --git a/controller/optimo_controller/optimo_task_gain_handler.hpp b/controller/optimo_controller/optimo_task_gain_handler.hpp index 84172278..f918f586 100644 --- a/controller/optimo_controller/optimo_task_gain_handler.hpp +++ b/controller/optimo_controller/optimo_task_gain_handler.hpp @@ -3,51 +3,50 @@ #include -namespace{ +namespace { constexpr int MAX_COUNT = 800; } class OptimoControlArchitecture; -class OptimoTaskGainHandler{ +class OptimoTaskGainHandler { public: - OptimoTaskGainHandler(OptimoControlArchitecture *ctrl_arch); - ~OptimoTaskGainHandler() = default; + OptimoTaskGainHandler(OptimoControlArchitecture *ctrl_arch); + ~OptimoTaskGainHandler() = default; - // for common tasks (1D, 2D, 3D .. etc) - void Trigger(const std::string &task_name, const Eigen::VectorXd &weight, - const Eigen::VectorXd &kp, const Eigen::VectorXd &kd); - // for com_xy task (icp task) - void Trigger(const std::string &task_name, const Eigen::VectorXd &weight, - const Eigen::VectorXd &kp, const Eigen::VectorXd &kd, - const Eigen::VectorXd &ki); + // for common tasks (1D, 2D, 3D .. etc) + void Trigger(const std::string &task_name, const Eigen::VectorXd &weight, + const Eigen::VectorXd &kp, const Eigen::VectorXd &kd); + // for com_xy task (icp task) + void Trigger(const std::string &task_name, const Eigen::VectorXd &weight, + const Eigen::VectorXd &kp, const Eigen::VectorXd &kd, + const Eigen::VectorXd &ki); - // process updating value with linear interpolation - void Process(); + // process updating value with linear interpolation + void Process(); - // getter - bool IsSignalReceived() { return b_signal_received_; } + // getter + bool IsSignalReceived() { return b_signal_received_; } private: - void _ResetParams(); - bool _TaskExists(const std::string &task_name); + void _ResetParams(); + bool _TaskExists(const std::string &task_name); - OptimoControlArchitecture *ctrl_arch_; - bool b_signal_received_; - bool b_first_visit_; - bool b_com_xy_task_; + OptimoControlArchitecture *ctrl_arch_; + bool b_signal_received_; + bool b_first_visit_; + bool b_com_xy_task_; - Eigen::VectorXd init_weight_; - Eigen::VectorXd init_kp_; - Eigen::VectorXd init_kd_; - Eigen::VectorXd init_ki_; + Eigen::VectorXd init_weight_; + Eigen::VectorXd init_kp_; + Eigen::VectorXd init_kd_; + Eigen::VectorXd init_ki_; - std::string task_name_; - Eigen::VectorXd target_weight_; - Eigen::VectorXd target_kp_; - Eigen::VectorXd target_kd_; - Eigen::VectorXd target_ki_; + std::string task_name_; + Eigen::VectorXd target_weight_; + Eigen::VectorXd target_kp_; + Eigen::VectorXd target_kd_; + Eigen::VectorXd target_ki_; int count_; - -}; \ No newline at end of file +}; diff --git a/controller/whole_body_controller/managers/arm_trajectory_manager.cpp b/controller/whole_body_controller/managers/arm_trajectory_manager.cpp index 661cda25..cb58d5e2 100644 --- a/controller/whole_body_controller/managers/arm_trajectory_manager.cpp +++ b/controller/whole_body_controller/managers/arm_trajectory_manager.cpp @@ -4,11 +4,10 @@ #include "util/interpolation.hpp" #include "util/util.hpp" -ArmTrajectoryManager::ArmTrajectoryManager( - Task *pos_task, Task *ori_task, PinocchioRobotSystem *robot) +ArmTrajectoryManager::ArmTrajectoryManager(Task *pos_task, Task *ori_task, + PinocchioRobotSystem *robot) : pos_task_(pos_task), ori_task_(ori_task), robot_(robot), - pos_curve_(nullptr), - ori_curve_(nullptr) { + pos_curve_(nullptr), ori_curve_(nullptr) { util::PrettyConstructor(2, "ArmTrajectoryManager"); } @@ -46,9 +45,11 @@ void ArmTrajectoryManager::UseNominal(const Eigen::Isometry3d &nominal_iso) { // transform quaternion to EigenXd to pass to Task Eigen::Quaterniond nominal_quat = Eigen::Quaterniond(nominal_iso.linear()); Eigen::Vector4d nominal_orientation; - nominal_orientation << nominal_quat.x(), nominal_quat.y(), nominal_quat.z(), nominal_quat.w(); + nominal_orientation << nominal_quat.x(), nominal_quat.y(), nominal_quat.z(), + nominal_quat.w(); - pos_task_->UpdateDesired(nominal_iso.translation(), zero_des_vel, zero_des_acc); + pos_task_->UpdateDesired(nominal_iso.translation(), zero_des_vel, + zero_des_acc); ori_task_->UpdateDesired(nominal_orientation, zero_des_vel, zero_des_acc); } @@ -64,20 +65,15 @@ void ArmTrajectoryManager::InitializeTrajectory( Eigen::VectorXd end_pos = Eigen::VectorXd::Zero(3); end_pos = fin_pose.translation(); + pos_curve_ = new HermiteCurveVec(start_pos, Eigen::VectorXd::Zero(3), end_pos, + Eigen::VectorXd::Zero(3), duration_); - pos_curve_ = - new HermiteCurveVec(start_pos, - Eigen::VectorXd::Zero(3), - end_pos, - Eigen::VectorXd::Zero(3), duration_); - - // pos_curve_ = new MinJerkCurveVec(start_pos, + // pos_curve_ = new MinJerkCurveVec(start_pos, + // Eigen::VectorXd::Zero(3), // Eigen::VectorXd::Zero(3), - // Eigen::VectorXd::Zero(3), // end_pos, - // Eigen::VectorXd::Zero(3), + // Eigen::VectorXd::Zero(3), // Eigen::VectorXd::Zero(3), duration_); - Eigen::Quaterniond start_ori(ini_pose.linear()); Eigen::Quaterniond end_ori(fin_pose.linear()); @@ -85,8 +81,6 @@ void ArmTrajectoryManager::InitializeTrajectory( ori_curve_ = new HermiteQuaternionCurve(start_ori, Eigen::Vector3d::Zero(), end_ori, Eigen::Vector3d::Zero(), duration_); - - } void ArmTrajectoryManager::UpdateDesired(const double current_time) { @@ -115,11 +109,7 @@ void ArmTrajectoryManager::UpdateDesired(const double current_time) { Eigen::VectorXd des_ori_acc(3); des_ori_acc << des_ori_acc_vec; - - - ori_task_->UpdateDesired(des_ori, des_ori_vel, des_ori_acc); counter_++; } - diff --git a/controller/whole_body_controller/managers/arm_trajectory_manager.hpp b/controller/whole_body_controller/managers/arm_trajectory_manager.hpp index fe7adab9..10d278d5 100644 --- a/controller/whole_body_controller/managers/arm_trajectory_manager.hpp +++ b/controller/whole_body_controller/managers/arm_trajectory_manager.hpp @@ -11,7 +11,7 @@ class MinJerkCurveQuat; class ArmTrajectoryManager { public: ArmTrajectoryManager(Task *pos_task, Task *ori_task, - PinocchioRobotSystem *robot); + PinocchioRobotSystem *robot); ~ArmTrajectoryManager(); void UseCurrent(); @@ -19,8 +19,8 @@ class ArmTrajectoryManager { void UseNominal(const Eigen::Isometry3d &nominal_iso); void InitializeTrajectory(const Eigen::Isometry3d &ini_pose, - const Eigen::Isometry3d &fin_pose, - const double duration); + const Eigen::Isometry3d &fin_pose, + const double duration); void UpdateDesired(const double current_time); private: diff --git a/scripts/README.md b/scripts/README.md index 1262ac4f..be798594 100644 --- a/scripts/README.md +++ b/scripts/README.md @@ -11,7 +11,7 @@ $ apt-get install libusb-1.0-0-dev xorg-dev libglu1-mesa-dev libglfw3 libglfw3-d $ git clone https://github.com/IntelRealSense/librealsense.git $ git checkout v2.53.1 -# Setting up authorizing USB devices (You may need "sudo" commands.) +# Setting up authorizing USB devices (You may need "sudo" commands.) $ cp config/99-realsense-libusb.rules /etc/udev/rules.d/ # Build and install the driver diff --git a/simulator/pybullet/draco_main.py b/simulator/pybullet/draco_main.py index ff8e0a43..4e0e79d4 100644 --- a/simulator/pybullet/draco_main.py +++ b/simulator/pybullet/draco_main.py @@ -44,8 +44,7 @@ def get_sensor_data_from_pybullet(robot): # follow pinocchio robotsystem urdf reading convention joint_pos, joint_vel = np.zeros(27), np.zeros(27) - imu_frame_quat = np.array( - pb.getLinkState(robot, DracoLinkIdx.torso_imu, 1, 1)[1]) + imu_frame_quat = np.array(pb.getLinkState(robot, DracoLinkIdx.torso_imu, 1, 1)[1]) # LF joint_pos[0] = pb.getJointState(robot, DracoJointIdx.l_hip_ie)[0] joint_pos[1] = pb.getJointState(robot, DracoJointIdx.l_hip_aa)[0] @@ -79,11 +78,11 @@ def get_sensor_data_from_pybullet(robot): joint_pos[25] = pb.getJointState(robot, DracoJointIdx.r_wrist_ps)[0] joint_pos[26] = pb.getJointState(robot, DracoJointIdx.r_wrist_pitch)[0] - imu_ang_vel = np.array( - pb.getLinkState(robot, DracoLinkIdx.torso_imu, 1, 1)[7]) + imu_ang_vel = np.array(pb.getLinkState(robot, DracoLinkIdx.torso_imu, 1, 1)[7]) - imu_dvel = pybullet_util.simulate_dVel_data(robot, DracoLinkIdx.torso_imu, - previous_torso_velocity) + imu_dvel = pybullet_util.simulate_dVel_data( + robot, DracoLinkIdx.torso_imu, previous_torso_velocity + ) # LF joint_vel[0] = pb.getJointState(robot, DracoJointIdx.l_hip_ie)[1] @@ -120,23 +119,27 @@ def get_sensor_data_from_pybullet(robot): # normal force measured on each foot _l_normal_force = 0 - contacts = pb.getContactPoints(bodyA=robot, - linkIndexA=DracoLinkIdx.l_ankle_ie_link) + contacts = pb.getContactPoints(bodyA=robot, linkIndexA=DracoLinkIdx.l_ankle_ie_link) for contact in contacts: # add z-component on all points of contact _l_normal_force += contact[9] _r_normal_force = 0 - contacts = pb.getContactPoints(bodyA=robot, - linkIndexA=DracoLinkIdx.r_ankle_ie_link) + contacts = pb.getContactPoints(bodyA=robot, linkIndexA=DracoLinkIdx.r_ankle_ie_link) for contact in contacts: # add z-component on all points of contact _r_normal_force += contact[9] - b_lf_contact = (True if pb.getLinkState(robot, DracoLinkIdx.l_foot_contact, - 1, 1)[0][2] <= 0.05 else False) - b_rf_contact = (True if pb.getLinkState(robot, DracoLinkIdx.r_foot_contact, - 1, 1)[0][2] <= 0.05 else False) + b_lf_contact = ( + True + if pb.getLinkState(robot, DracoLinkIdx.l_foot_contact, 1, 1)[0][2] <= 0.05 + else False + ) + b_rf_contact = ( + True + if pb.getLinkState(robot, DracoLinkIdx.r_foot_contact, 1, 1)[0][2] <= 0.05 + else False + ) return ( imu_frame_quat, imu_ang_vel, @@ -155,116 +158,91 @@ def apply_control_input_to_pybullet(robot, command): mode = pb.TORQUE_CONTROL # LF - pb.setJointMotorControl2(robot, - DracoJointIdx.l_hip_ie, - controlMode=mode, - force=command[0]) - pb.setJointMotorControl2(robot, - DracoJointIdx.l_hip_aa, - controlMode=mode, - force=command[1]) - pb.setJointMotorControl2(robot, - DracoJointIdx.l_hip_fe, - controlMode=mode, - force=command[2]) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_hip_ie, controlMode=mode, force=command[0] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_hip_aa, controlMode=mode, force=command[1] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_hip_fe, controlMode=mode, force=command[2] + ) # pb.setJointMotorControl2(robot, DracoJointIdx.l_knee_fe_jp, controlMode=mode, force=command[3]) - pb.setJointMotorControl2(robot, - DracoJointIdx.l_knee_fe_jd, - controlMode=mode, - force=command[4]) - pb.setJointMotorControl2(robot, - DracoJointIdx.l_ankle_fe, - controlMode=mode, - force=command[5]) - pb.setJointMotorControl2(robot, - DracoJointIdx.l_ankle_ie, - controlMode=mode, - force=command[6]) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_knee_fe_jd, controlMode=mode, force=command[4] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_ankle_fe, controlMode=mode, force=command[5] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_ankle_ie, controlMode=mode, force=command[6] + ) # LH - pb.setJointMotorControl2(robot, - DracoJointIdx.l_shoulder_fe, - controlMode=mode, - force=command[7]) - pb.setJointMotorControl2(robot, - DracoJointIdx.l_shoulder_aa, - controlMode=mode, - force=command[8]) - pb.setJointMotorControl2(robot, - DracoJointIdx.l_shoulder_ie, - controlMode=mode, - force=command[9]) - pb.setJointMotorControl2(robot, - DracoJointIdx.l_elbow_fe, - controlMode=mode, - force=command[10]) - pb.setJointMotorControl2(robot, - DracoJointIdx.l_wrist_ps, - controlMode=mode, - force=command[11]) - pb.setJointMotorControl2(robot, - DracoJointIdx.l_wrist_pitch, - controlMode=mode, - force=command[12]) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_shoulder_fe, controlMode=mode, force=command[7] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_shoulder_aa, controlMode=mode, force=command[8] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_shoulder_ie, controlMode=mode, force=command[9] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_elbow_fe, controlMode=mode, force=command[10] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_wrist_ps, controlMode=mode, force=command[11] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.l_wrist_pitch, controlMode=mode, force=command[12] + ) # neck - pb.setJointMotorControl2(robot, - DracoJointIdx.neck_pitch, - controlMode=mode, - force=command[13]) + pb.setJointMotorControl2( + robot, DracoJointIdx.neck_pitch, controlMode=mode, force=command[13] + ) # RF - pb.setJointMotorControl2(robot, - DracoJointIdx.r_hip_ie, - controlMode=mode, - force=command[14]) - pb.setJointMotorControl2(robot, - DracoJointIdx.r_hip_aa, - controlMode=mode, - force=command[15]) - pb.setJointMotorControl2(robot, - DracoJointIdx.r_hip_fe, - controlMode=mode, - force=command[16]) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_hip_ie, controlMode=mode, force=command[14] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_hip_aa, controlMode=mode, force=command[15] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_hip_fe, controlMode=mode, force=command[16] + ) # pb.setJointMotorControl2(robot, DracoJointIdx.r_knee_fe_jd, controlMode=mode, force=command[17]) - pb.setJointMotorControl2(robot, - DracoJointIdx.r_knee_fe_jd, - controlMode=mode, - force=command[18]) - pb.setJointMotorControl2(robot, - DracoJointIdx.r_ankle_fe, - controlMode=mode, - force=command[19]) - pb.setJointMotorControl2(robot, - DracoJointIdx.r_ankle_ie, - controlMode=mode, - force=command[20]) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_knee_fe_jd, controlMode=mode, force=command[18] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_ankle_fe, controlMode=mode, force=command[19] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_ankle_ie, controlMode=mode, force=command[20] + ) # RH - pb.setJointMotorControl2(robot, - DracoJointIdx.r_shoulder_fe, - controlMode=mode, - force=command[21]) - pb.setJointMotorControl2(robot, - DracoJointIdx.r_shoulder_aa, - controlMode=mode, - force=command[22]) - pb.setJointMotorControl2(robot, - DracoJointIdx.r_shoulder_ie, - controlMode=mode, - force=command[23]) - pb.setJointMotorControl2(robot, - DracoJointIdx.r_elbow_fe, - controlMode=mode, - force=command[24]) - pb.setJointMotorControl2(robot, - DracoJointIdx.r_wrist_ps, - controlMode=mode, - force=command[25]) - pb.setJointMotorControl2(robot, - DracoJointIdx.r_wrist_pitch, - controlMode=mode, - force=command[26]) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_shoulder_fe, controlMode=mode, force=command[21] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_shoulder_aa, controlMode=mode, force=command[22] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_shoulder_ie, controlMode=mode, force=command[23] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_elbow_fe, controlMode=mode, force=command[24] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_wrist_ps, controlMode=mode, force=command[25] + ) + pb.setJointMotorControl2( + robot, DracoJointIdx.r_wrist_pitch, controlMode=mode, force=command[26] + ) def set_init_config_pybullet_robot(robot): @@ -276,23 +254,19 @@ def set_init_config_pybullet_robot(robot): # Lowerbody hip_yaw_angle = 0 - pb.resetJointState(robot, DracoJointIdx.l_hip_aa, - np.radians(hip_yaw_angle), 0.0) + pb.resetJointState(robot, DracoJointIdx.l_hip_aa, np.radians(hip_yaw_angle), 0.0) pb.resetJointState(robot, DracoJointIdx.l_hip_fe, -np.pi / 4, 0.0) pb.resetJointState(robot, DracoJointIdx.l_knee_fe_jp, np.pi / 4, 0.0) pb.resetJointState(robot, DracoJointIdx.l_knee_fe_jd, np.pi / 4, 0.0) pb.resetJointState(robot, DracoJointIdx.l_ankle_fe, -np.pi / 4, 0.0) - pb.resetJointState(robot, DracoJointIdx.l_ankle_ie, - np.radians(-hip_yaw_angle), 0.0) + pb.resetJointState(robot, DracoJointIdx.l_ankle_ie, np.radians(-hip_yaw_angle), 0.0) - pb.resetJointState(robot, DracoJointIdx.r_hip_aa, - np.radians(-hip_yaw_angle), 0.0) + pb.resetJointState(robot, DracoJointIdx.r_hip_aa, np.radians(-hip_yaw_angle), 0.0) pb.resetJointState(robot, DracoJointIdx.r_hip_fe, -np.pi / 4, 0.0) pb.resetJointState(robot, DracoJointIdx.r_knee_fe_jp, np.pi / 4, 0.0) pb.resetJointState(robot, DracoJointIdx.r_knee_fe_jd, np.pi / 4, 0.0) pb.resetJointState(robot, DracoJointIdx.r_ankle_fe, -np.pi / 4, 0.0) - pb.resetJointState(robot, DracoJointIdx.r_ankle_ie, - np.radians(hip_yaw_angle), 0.0) + pb.resetJointState(robot, DracoJointIdx.r_ankle_ie, np.radians(hip_yaw_angle), 0.0) def signal_handler(signal, frame): @@ -302,9 +276,9 @@ def signal_handler(signal, frame): print("========================================================") print('saving list of compuation time in "compuation_time.txt"') print("========================================================") - np.savetxt("computation_time.txt", - np.array([compuation_cal_list]), - delimiter=",") + np.savetxt( + "computation_time.txt", np.array([compuation_cal_list]), delimiter="," + ) if Config.VIDEO_RECORD: print("========================================================") @@ -330,8 +304,9 @@ def signal_handler(signal, frame): cameraTargetPosition=[0, 0, 0.5], ) ## sim physics setting - pb.setPhysicsEngineParameter(fixedTimeStep=Config.CONTROLLER_DT, - numSubSteps=Config.N_SUBSTEP) + pb.setPhysicsEngineParameter( + fixedTimeStep=Config.CONTROLLER_DT, numSubSteps=Config.N_SUBSTEP + ) pb.setGravity(0, 0, -9.81) ## robot spawn & initial kinematics and dynamics setting @@ -347,8 +322,7 @@ def signal_handler(signal, frame): useFixedBase=0, ) - ground = pb.loadURDF(cwd + "/robot_model/ground/plane.urdf", - useFixedBase=1) + ground = pb.loadURDF(cwd + "/robot_model/ground/plane.urdf", useFixedBase=1) pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) # TODO:modify this function without dictionary container @@ -411,18 +385,17 @@ def signal_handler(signal, frame): active_jointidx_list = [] # for setjointmotorcontrolarray # default robot kinematics information - base_com_pos, base_com_quat = pb.getBasePositionAndOrientation( - draco_humanoid) + base_com_pos, base_com_quat = pb.getBasePositionAndOrientation(draco_humanoid) rot_world_basecom = util.quat_to_rot(np.array(base_com_quat)) - rot_world_basejoint = util.quat_to_rot( - np.array(Config.INITIAL_BASE_JOINT_QUAT)) + rot_world_basejoint = util.quat_to_rot(np.array(Config.INITIAL_BASE_JOINT_QUAT)) pos_basejoint_to_basecom = np.dot( rot_world_basejoint.transpose(), base_com_pos - np.array(Config.INITIAL_BASE_JOINT_POS), ) - rot_basejoint_to_basecom = np.dot(rot_world_basejoint.transpose(), - rot_world_basecom) + rot_basejoint_to_basecom = np.dot( + rot_world_basejoint.transpose(), rot_world_basecom + ) # Run Simulation dt = Config.CONTROLLER_DT @@ -456,26 +429,27 @@ def signal_handler(signal, frame): cameraDistance=1.5, cameraYaw=120, cameraPitch=-30, - cameraTargetPosition=base_pos + - np.array([0.5, 0.3, -base_pos[2] + 1]), + cameraTargetPosition=base_pos + np.array([0.5, 0.3, -base_pos[2] + 1]), ) ############################################################################### # Debugging Purpose ############################################################################## ##debugging state estimator by calculating groundtruth basejoint states - base_com_pos, base_com_quat = pb.getBasePositionAndOrientation( - draco_humanoid) + base_com_pos, base_com_quat = pb.getBasePositionAndOrientation(draco_humanoid) rot_world_basecom = util.quat_to_rot(base_com_quat) - rot_world_basejoint = np.dot(rot_world_basecom, - rot_basejoint_to_basecom.transpose()) - base_joint_pos = base_com_pos - np.dot(rot_world_basejoint, - pos_basejoint_to_basecom) + rot_world_basejoint = np.dot( + rot_world_basecom, rot_basejoint_to_basecom.transpose() + ) + base_joint_pos = base_com_pos - np.dot( + rot_world_basejoint, pos_basejoint_to_basecom + ) base_joint_quat = util.rot_to_quat(rot_world_basejoint) base_com_lin_vel, base_com_ang_vel = pb.getBaseVelocity(draco_humanoid) - trans_joint_com = liegroup.RpToTrans(rot_basejoint_to_basecom, - pos_basejoint_to_basecom) + trans_joint_com = liegroup.RpToTrans( + rot_basejoint_to_basecom, pos_basejoint_to_basecom + ) adjoint_joint_com = liegroup.Adjoint(trans_joint_com) twist_basecom_in_world = np.zeros(6) twist_basecom_in_world[0:3] = base_com_ang_vel @@ -483,15 +457,16 @@ def signal_handler(signal, frame): augrot_basecom_world = np.zeros((6, 6)) augrot_basecom_world[0:3, 0:3] = rot_world_basecom.transpose() augrot_basecom_world[3:6, 3:6] = rot_world_basecom.transpose() - twist_basecom_in_basecom = np.dot(augrot_basecom_world, - twist_basecom_in_world) - twist_basejoint_in_basejoint = np.dot(adjoint_joint_com, - twist_basecom_in_basecom) + twist_basecom_in_basecom = np.dot(augrot_basecom_world, twist_basecom_in_world) + twist_basejoint_in_basejoint = np.dot( + adjoint_joint_com, twist_basecom_in_basecom + ) augrot_world_basejoint = np.zeros((6, 6)) augrot_world_basejoint[0:3, 0:3] = rot_world_basejoint augrot_world_basejoint[3:6, 3:6] = rot_world_basejoint - twist_basejoint_in_world = np.dot(augrot_world_basejoint, - twist_basejoint_in_basejoint) + twist_basejoint_in_world = np.dot( + augrot_world_basejoint, twist_basejoint_in_basejoint + ) base_joint_ang_vel = twist_basejoint_in_world[0:3] base_joint_lin_vel = twist_basejoint_in_world[3:6] @@ -545,12 +520,13 @@ def signal_handler(signal, frame): l_normal_force = pybullet_util.simulate_contact_sensor(l_normal_force) r_normal_force = pybullet_util.simulate_contact_sensor(r_normal_force) imu_dvel = pybullet_util.add_sensor_noise(imu_dvel, imu_dvel_bias) - imu_ang_vel = pybullet_util.add_sensor_noise(imu_ang_vel, - imu_ang_vel_noise) + imu_ang_vel = pybullet_util.add_sensor_noise(imu_ang_vel, imu_ang_vel_noise) l_normal_force = pybullet_util.add_sensor_noise( - l_normal_force, l_normal_volt_noise) + l_normal_force, l_normal_volt_noise + ) r_normal_force = pybullet_util.add_sensor_noise( - r_normal_force, r_normal_volt_noise) + r_normal_force, r_normal_volt_noise + ) # copy sensor data to rpc sensor data class rpc_draco_sensor_data.imu_frame_quat_ = imu_frame_quat @@ -570,8 +546,7 @@ def signal_handler(signal, frame): if Config.MEASURE_COMPUTATION_TIME: timer.tic() - rpc_draco_interface.GetCommand(rpc_draco_sensor_data, - rpc_draco_command) + rpc_draco_interface.GetCommand(rpc_draco_sensor_data, rpc_draco_command) if Config.MEASURE_COMPUTATION_TIME: comp_time = timer.tocvalue() @@ -588,7 +563,8 @@ def signal_handler(signal, frame): # lfoot_pos = pybullet_util.get_link_iso(draco_humanoid, # save current torso velocity for next iteration previous_torso_velocity = pybullet_util.get_link_vel( - draco_humanoid, link_id_dict["torso_imu"])[3:6] + draco_humanoid, link_id_dict["torso_imu"] + )[3:6] # DracoLinkIdx.l_foot_contact)[0:3, 3] # rfoot_pos = pybullet_util.get_link_iso(draco_humanoid, @@ -607,7 +583,8 @@ def signal_handler(signal, frame): if (Config.VIDEO_RECORD) and (count % Config.RECORD_FREQ == 0): camera_data = pb.getDebugVisualizerCamera() frame = pybullet_util.get_camera_image_from_debug_camera( - camera_data, Config.RENDER_WIDTH, Config.RENDER_HEIGHT) + camera_data, Config.RENDER_WIDTH, Config.RENDER_HEIGHT + ) filename = video_dir + "/step%06d.jpg" % jpg_count cv2.imwrite(filename, frame) jpg_count += 1 diff --git a/simulator/pybullet/draco_main_wbic.py b/simulator/pybullet/draco_main_wbic.py index 97bc3742..8ba7f891 100644 --- a/simulator/pybullet/draco_main_wbic.py +++ b/simulator/pybullet/draco_main_wbic.py @@ -30,122 +30,89 @@ def get_sensor_data_from_pybullet(robot): # follow pinocchio robotsystem urdf reading convention joint_pos, joint_vel = np.zeros(27), np.zeros(27) - imu_frame_quat = np.array( - pb.getLinkState(robot, DracoLinkIdx.torso_imu, 1, 1)[1]) + imu_frame_quat = np.array(pb.getLinkState(robot, DracoLinkIdx.torso_imu, 1, 1)[1]) # LF joint_pos[0] = pb.getJointState(robot, PybulletDracoJointIdx.l_hip_ie)[0] joint_pos[1] = pb.getJointState(robot, PybulletDracoJointIdx.l_hip_aa)[0] joint_pos[2] = pb.getJointState(robot, PybulletDracoJointIdx.l_hip_fe)[0] - joint_pos[3] = pb.getJointState(robot, - PybulletDracoJointIdx.l_knee_fe_jp)[0] - joint_pos[4] = pb.getJointState(robot, - PybulletDracoJointIdx.l_knee_fe_jd)[0] + joint_pos[3] = pb.getJointState(robot, PybulletDracoJointIdx.l_knee_fe_jp)[0] + joint_pos[4] = pb.getJointState(robot, PybulletDracoJointIdx.l_knee_fe_jd)[0] joint_pos[5] = pb.getJointState(robot, PybulletDracoJointIdx.l_ankle_fe)[0] joint_pos[6] = pb.getJointState(robot, PybulletDracoJointIdx.l_ankle_ie)[0] # LH - joint_pos[7] = pb.getJointState(robot, - PybulletDracoJointIdx.l_shoulder_fe)[0] - joint_pos[8] = pb.getJointState(robot, - PybulletDracoJointIdx.l_shoulder_aa)[0] - joint_pos[9] = pb.getJointState(robot, - PybulletDracoJointIdx.l_shoulder_ie)[0] - joint_pos[10] = pb.getJointState(robot, - PybulletDracoJointIdx.l_elbow_fe)[0] - joint_pos[11] = pb.getJointState(robot, - PybulletDracoJointIdx.l_wrist_ps)[0] - joint_pos[12] = pb.getJointState(robot, - PybulletDracoJointIdx.l_wrist_pitch)[0] + joint_pos[7] = pb.getJointState(robot, PybulletDracoJointIdx.l_shoulder_fe)[0] + joint_pos[8] = pb.getJointState(robot, PybulletDracoJointIdx.l_shoulder_aa)[0] + joint_pos[9] = pb.getJointState(robot, PybulletDracoJointIdx.l_shoulder_ie)[0] + joint_pos[10] = pb.getJointState(robot, PybulletDracoJointIdx.l_elbow_fe)[0] + joint_pos[11] = pb.getJointState(robot, PybulletDracoJointIdx.l_wrist_ps)[0] + joint_pos[12] = pb.getJointState(robot, PybulletDracoJointIdx.l_wrist_pitch)[0] # neck - joint_pos[13] = pb.getJointState(robot, - PybulletDracoJointIdx.neck_pitch)[0] + joint_pos[13] = pb.getJointState(robot, PybulletDracoJointIdx.neck_pitch)[0] # RF joint_pos[14] = pb.getJointState(robot, PybulletDracoJointIdx.r_hip_ie)[0] joint_pos[15] = pb.getJointState(robot, PybulletDracoJointIdx.r_hip_aa)[0] joint_pos[16] = pb.getJointState(robot, PybulletDracoJointIdx.r_hip_fe)[0] - joint_pos[17] = pb.getJointState(robot, - PybulletDracoJointIdx.r_knee_fe_jp)[0] - joint_pos[18] = pb.getJointState(robot, - PybulletDracoJointIdx.r_knee_fe_jd)[0] - joint_pos[19] = pb.getJointState(robot, - PybulletDracoJointIdx.r_ankle_fe)[0] - joint_pos[20] = pb.getJointState(robot, - PybulletDracoJointIdx.r_ankle_ie)[0] + joint_pos[17] = pb.getJointState(robot, PybulletDracoJointIdx.r_knee_fe_jp)[0] + joint_pos[18] = pb.getJointState(robot, PybulletDracoJointIdx.r_knee_fe_jd)[0] + joint_pos[19] = pb.getJointState(robot, PybulletDracoJointIdx.r_ankle_fe)[0] + joint_pos[20] = pb.getJointState(robot, PybulletDracoJointIdx.r_ankle_ie)[0] # RH - joint_pos[21] = pb.getJointState(robot, - PybulletDracoJointIdx.r_shoulder_fe)[0] - joint_pos[22] = pb.getJointState(robot, - PybulletDracoJointIdx.r_shoulder_aa)[0] - joint_pos[23] = pb.getJointState(robot, - PybulletDracoJointIdx.r_shoulder_ie)[0] - joint_pos[24] = pb.getJointState(robot, - PybulletDracoJointIdx.r_elbow_fe)[0] - joint_pos[25] = pb.getJointState(robot, - PybulletDracoJointIdx.r_wrist_ps)[0] - joint_pos[26] = pb.getJointState(robot, - PybulletDracoJointIdx.r_wrist_pitch)[0] - - imu_ang_vel = np.array( - pb.getLinkState(robot, DracoLinkIdx.torso_imu, 1, 1)[7]) - - imu_dvel = pybullet_util.simulate_dVel_data(robot, DracoLinkIdx.torso_imu, - previous_torso_velocity) + joint_pos[21] = pb.getJointState(robot, PybulletDracoJointIdx.r_shoulder_fe)[0] + joint_pos[22] = pb.getJointState(robot, PybulletDracoJointIdx.r_shoulder_aa)[0] + joint_pos[23] = pb.getJointState(robot, PybulletDracoJointIdx.r_shoulder_ie)[0] + joint_pos[24] = pb.getJointState(robot, PybulletDracoJointIdx.r_elbow_fe)[0] + joint_pos[25] = pb.getJointState(robot, PybulletDracoJointIdx.r_wrist_ps)[0] + joint_pos[26] = pb.getJointState(robot, PybulletDracoJointIdx.r_wrist_pitch)[0] + + imu_ang_vel = np.array(pb.getLinkState(robot, DracoLinkIdx.torso_imu, 1, 1)[7]) + + imu_dvel = pybullet_util.simulate_dVel_data( + robot, DracoLinkIdx.torso_imu, previous_torso_velocity + ) # LF joint_vel[0] = pb.getJointState(robot, PybulletDracoJointIdx.l_hip_ie)[1] joint_vel[1] = pb.getJointState(robot, PybulletDracoJointIdx.l_hip_aa)[1] joint_vel[2] = pb.getJointState(robot, PybulletDracoJointIdx.l_hip_fe)[1] - joint_vel[3] = pb.getJointState(robot, - PybulletDracoJointIdx.l_knee_fe_jp)[1] - joint_vel[4] = pb.getJointState(robot, - PybulletDracoJointIdx.l_knee_fe_jd)[1] + joint_vel[3] = pb.getJointState(robot, PybulletDracoJointIdx.l_knee_fe_jp)[1] + joint_vel[4] = pb.getJointState(robot, PybulletDracoJointIdx.l_knee_fe_jd)[1] joint_vel[5] = pb.getJointState(robot, PybulletDracoJointIdx.l_ankle_fe)[1] joint_vel[6] = pb.getJointState(robot, PybulletDracoJointIdx.l_ankle_ie)[1] # LH - joint_vel[7] = pb.getJointState(robot, - PybulletDracoJointIdx.l_shoulder_fe)[1] - joint_vel[8] = pb.getJointState(robot, - PybulletDracoJointIdx.l_shoulder_aa)[1] - joint_vel[9] = pb.getJointState(robot, - PybulletDracoJointIdx.l_shoulder_ie)[1] - joint_vel[10] = pb.getJointState(robot, - PybulletDracoJointIdx.l_elbow_fe)[1] - joint_vel[11] = pb.getJointState(robot, - PybulletDracoJointIdx.l_wrist_ps)[1] - joint_vel[12] = pb.getJointState(robot, - PybulletDracoJointIdx.l_wrist_pitch)[1] + joint_vel[7] = pb.getJointState(robot, PybulletDracoJointIdx.l_shoulder_fe)[1] + joint_vel[8] = pb.getJointState(robot, PybulletDracoJointIdx.l_shoulder_aa)[1] + joint_vel[9] = pb.getJointState(robot, PybulletDracoJointIdx.l_shoulder_ie)[1] + joint_vel[10] = pb.getJointState(robot, PybulletDracoJointIdx.l_elbow_fe)[1] + joint_vel[11] = pb.getJointState(robot, PybulletDracoJointIdx.l_wrist_ps)[1] + joint_vel[12] = pb.getJointState(robot, PybulletDracoJointIdx.l_wrist_pitch)[1] # neck - joint_vel[13] = pb.getJointState(robot, - PybulletDracoJointIdx.neck_pitch)[1] + joint_vel[13] = pb.getJointState(robot, PybulletDracoJointIdx.neck_pitch)[1] # RF joint_vel[14] = pb.getJointState(robot, PybulletDracoJointIdx.r_hip_ie)[1] joint_vel[15] = pb.getJointState(robot, PybulletDracoJointIdx.r_hip_aa)[1] joint_vel[16] = pb.getJointState(robot, PybulletDracoJointIdx.r_hip_fe)[1] - joint_vel[17] = pb.getJointState(robot, - PybulletDracoJointIdx.r_knee_fe_jp)[1] - joint_vel[18] = pb.getJointState(robot, - PybulletDracoJointIdx.r_knee_fe_jd)[1] - joint_vel[19] = pb.getJointState(robot, - PybulletDracoJointIdx.r_ankle_fe)[1] - joint_vel[20] = pb.getJointState(robot, - PybulletDracoJointIdx.r_ankle_ie)[1] + joint_vel[17] = pb.getJointState(robot, PybulletDracoJointIdx.r_knee_fe_jp)[1] + joint_vel[18] = pb.getJointState(robot, PybulletDracoJointIdx.r_knee_fe_jd)[1] + joint_vel[19] = pb.getJointState(robot, PybulletDracoJointIdx.r_ankle_fe)[1] + joint_vel[20] = pb.getJointState(robot, PybulletDracoJointIdx.r_ankle_ie)[1] # RH - joint_vel[21] = pb.getJointState(robot, - PybulletDracoJointIdx.r_shoulder_fe)[1] - joint_vel[22] = pb.getJointState(robot, - PybulletDracoJointIdx.r_shoulder_aa)[1] - joint_vel[23] = pb.getJointState(robot, - PybulletDracoJointIdx.r_shoulder_ie)[1] - joint_vel[24] = pb.getJointState(robot, - PybulletDracoJointIdx.r_elbow_fe)[1] - joint_vel[25] = pb.getJointState(robot, - PybulletDracoJointIdx.r_wrist_ps)[1] - joint_vel[26] = pb.getJointState(robot, - PybulletDracoJointIdx.r_wrist_pitch)[1] - - b_lf_contact = (True if pb.getLinkState(robot, DracoLinkIdx.l_foot_contact, - 1, 1)[0][2] <= 0.05 else False) - b_rf_contact = (True if pb.getLinkState(robot, DracoLinkIdx.r_foot_contact, - 1, 1)[0][2] <= 0.05 else False) + joint_vel[21] = pb.getJointState(robot, PybulletDracoJointIdx.r_shoulder_fe)[1] + joint_vel[22] = pb.getJointState(robot, PybulletDracoJointIdx.r_shoulder_aa)[1] + joint_vel[23] = pb.getJointState(robot, PybulletDracoJointIdx.r_shoulder_ie)[1] + joint_vel[24] = pb.getJointState(robot, PybulletDracoJointIdx.r_elbow_fe)[1] + joint_vel[25] = pb.getJointState(robot, PybulletDracoJointIdx.r_wrist_ps)[1] + joint_vel[26] = pb.getJointState(robot, PybulletDracoJointIdx.r_wrist_pitch)[1] + + b_lf_contact = ( + True + if pb.getLinkState(robot, DracoLinkIdx.l_foot_contact, 1, 1)[0][2] <= 0.05 + else False + ) + b_rf_contact = ( + True + if pb.getLinkState(robot, DracoLinkIdx.r_foot_contact, 1, 1)[0][2] <= 0.05 + else False + ) return ( imu_frame_quat, imu_ang_vel, @@ -162,120 +129,94 @@ def apply_torque_control_to_pybullet(robot, command): mode = pb.TORQUE_CONTROL # LF - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_hip_ie, - controlMode=mode, - force=command[0]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_hip_aa, - controlMode=mode, - force=command[1]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_hip_fe, - controlMode=mode, - force=command[2]) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_hip_ie, controlMode=mode, force=command[0] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_hip_aa, controlMode=mode, force=command[1] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_hip_fe, controlMode=mode, force=command[2] + ) # pb.setJointMotorControl2(robot, PybulletDracoJointIdx.l_knee_fe_jp, controlMode=mode, force=command[3]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_knee_fe_jd, - controlMode=mode, - force=command[4]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_ankle_fe, - controlMode=mode, - force=command[5]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_ankle_ie, - controlMode=mode, - force=command[6]) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_knee_fe_jd, controlMode=mode, force=command[4] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_ankle_fe, controlMode=mode, force=command[5] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_ankle_ie, controlMode=mode, force=command[6] + ) # LH - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_shoulder_fe, - controlMode=mode, - force=command[7]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_shoulder_aa, - controlMode=mode, - force=command[8]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_shoulder_ie, - controlMode=mode, - force=command[9]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_elbow_fe, - controlMode=mode, - force=command[10]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_wrist_ps, - controlMode=mode, - force=command[11]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.l_wrist_pitch, - controlMode=mode, - force=command[12]) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_shoulder_fe, controlMode=mode, force=command[7] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_shoulder_aa, controlMode=mode, force=command[8] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_shoulder_ie, controlMode=mode, force=command[9] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_elbow_fe, controlMode=mode, force=command[10] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_wrist_ps, controlMode=mode, force=command[11] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.l_wrist_pitch, controlMode=mode, force=command[12] + ) # neck - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.neck_pitch, - controlMode=mode, - force=command[13]) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.neck_pitch, controlMode=mode, force=command[13] + ) # RF - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_hip_ie, - controlMode=mode, - force=command[14]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_hip_aa, - controlMode=mode, - force=command[15]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_hip_fe, - controlMode=mode, - force=command[16]) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_hip_ie, controlMode=mode, force=command[14] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_hip_aa, controlMode=mode, force=command[15] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_hip_fe, controlMode=mode, force=command[16] + ) # pb.setJointMotorControl2(robot, PybulletDracoJointIdx.r_knee_fe_jd, controlMode=mode, force=command[17]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_knee_fe_jd, - controlMode=mode, - force=command[18]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_ankle_fe, - controlMode=mode, - force=command[19]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_ankle_ie, - controlMode=mode, - force=command[20]) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_knee_fe_jd, controlMode=mode, force=command[18] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_ankle_fe, controlMode=mode, force=command[19] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_ankle_ie, controlMode=mode, force=command[20] + ) # RH - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_shoulder_fe, - controlMode=mode, - force=command[21]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_shoulder_aa, - controlMode=mode, - force=command[22]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_shoulder_ie, - controlMode=mode, - force=command[23]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_elbow_fe, - controlMode=mode, - force=command[24]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_wrist_ps, - controlMode=mode, - force=command[25]) - pb.setJointMotorControl2(robot, - PybulletDracoJointIdx.r_wrist_pitch, - controlMode=mode, - force=command[26]) - - -def apply_position_control_to_pybullet(robot, pos_command, vel_command, kp, - kd): + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_shoulder_fe, controlMode=mode, force=command[21] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_shoulder_aa, controlMode=mode, force=command[22] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_shoulder_ie, controlMode=mode, force=command[23] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_elbow_fe, controlMode=mode, force=command[24] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_wrist_ps, controlMode=mode, force=command[25] + ) + pb.setJointMotorControl2( + robot, PybulletDracoJointIdx.r_wrist_pitch, controlMode=mode, force=command[26] + ) + + +def apply_position_control_to_pybullet(robot, pos_command, vel_command, kp, kd): mode = pb.POSITION_CONTROL # LF @@ -518,40 +459,34 @@ def apply_position_control_to_pybullet(robot, pos_command, vel_command, kp, def set_init_config_pybullet_robot(robot): # Upperbody - pb.resetJointState(robot, PybulletDracoJointIdx.l_shoulder_aa, np.pi / 6, - 0.0) - pb.resetJointState(robot, PybulletDracoJointIdx.l_elbow_fe, -np.pi / 2, - 0.0) - pb.resetJointState(robot, PybulletDracoJointIdx.r_shoulder_aa, -np.pi / 6, - 0.0) - pb.resetJointState(robot, PybulletDracoJointIdx.r_elbow_fe, -np.pi / 2, - 0.0) + pb.resetJointState(robot, PybulletDracoJointIdx.l_shoulder_aa, np.pi / 6, 0.0) + pb.resetJointState(robot, PybulletDracoJointIdx.l_elbow_fe, -np.pi / 2, 0.0) + pb.resetJointState(robot, PybulletDracoJointIdx.r_shoulder_aa, -np.pi / 6, 0.0) + pb.resetJointState(robot, PybulletDracoJointIdx.r_elbow_fe, -np.pi / 2, 0.0) # Lowerbody hip_yaw_angle = 0 - pb.resetJointState(robot, PybulletDracoJointIdx.l_hip_aa, - np.radians(hip_yaw_angle), 0.0) + pb.resetJointState( + robot, PybulletDracoJointIdx.l_hip_aa, np.radians(hip_yaw_angle), 0.0 + ) pb.resetJointState(robot, PybulletDracoJointIdx.l_hip_fe, -np.pi / 4, 0.0) - pb.resetJointState(robot, PybulletDracoJointIdx.l_knee_fe_jp, np.pi / 4, - 0.0) - pb.resetJointState(robot, PybulletDracoJointIdx.l_knee_fe_jd, np.pi / 4, - 0.0) - pb.resetJointState(robot, PybulletDracoJointIdx.l_ankle_fe, -np.pi / 4, - 0.0) - pb.resetJointState(robot, PybulletDracoJointIdx.l_ankle_ie, - np.radians(-hip_yaw_angle), 0.0) - - pb.resetJointState(robot, PybulletDracoJointIdx.r_hip_aa, - np.radians(-hip_yaw_angle), 0.0) + pb.resetJointState(robot, PybulletDracoJointIdx.l_knee_fe_jp, np.pi / 4, 0.0) + pb.resetJointState(robot, PybulletDracoJointIdx.l_knee_fe_jd, np.pi / 4, 0.0) + pb.resetJointState(robot, PybulletDracoJointIdx.l_ankle_fe, -np.pi / 4, 0.0) + pb.resetJointState( + robot, PybulletDracoJointIdx.l_ankle_ie, np.radians(-hip_yaw_angle), 0.0 + ) + + pb.resetJointState( + robot, PybulletDracoJointIdx.r_hip_aa, np.radians(-hip_yaw_angle), 0.0 + ) pb.resetJointState(robot, PybulletDracoJointIdx.r_hip_fe, -np.pi / 4, 0.0) - pb.resetJointState(robot, PybulletDracoJointIdx.r_knee_fe_jp, np.pi / 4, - 0.0) - pb.resetJointState(robot, PybulletDracoJointIdx.r_knee_fe_jd, np.pi / 4, - 0.0) - pb.resetJointState(robot, PybulletDracoJointIdx.r_ankle_fe, -np.pi / 4, - 0.0) - pb.resetJointState(robot, PybulletDracoJointIdx.r_ankle_ie, - np.radians(hip_yaw_angle), 0.0) + pb.resetJointState(robot, PybulletDracoJointIdx.r_knee_fe_jp, np.pi / 4, 0.0) + pb.resetJointState(robot, PybulletDracoJointIdx.r_knee_fe_jd, np.pi / 4, 0.0) + pb.resetJointState(robot, PybulletDracoJointIdx.r_ankle_fe, -np.pi / 4, 0.0) + pb.resetJointState( + robot, PybulletDracoJointIdx.r_ankle_ie, np.radians(hip_yaw_angle), 0.0 + ) def signal_handler(signal, frame): @@ -561,9 +496,9 @@ def signal_handler(signal, frame): print("========================================================") print('saving list of compuation time in "compuation_time.txt"') print("========================================================") - np.savetxt("computation_time.txt", - np.array([compuation_cal_list]), - delimiter=",") + np.savetxt( + "computation_time.txt", np.array([compuation_cal_list]), delimiter="," + ) if Config.VIDEO_RECORD: print("========================================================") @@ -589,8 +524,9 @@ def signal_handler(signal, frame): cameraTargetPosition=[0, 0, 0.5], ) ## sim physics setting - pb.setPhysicsEngineParameter(fixedTimeStep=Config.CONTROLLER_DT, - numSubSteps=Config.N_SUBSTEP) + pb.setPhysicsEngineParameter( + fixedTimeStep=Config.CONTROLLER_DT, numSubSteps=Config.N_SUBSTEP + ) pb.setGravity(0, 0, -9.81) ## robot spawn & initial kinematics and dynamics setting @@ -608,8 +544,7 @@ def signal_handler(signal, frame): # Config.INITIAL_BASE_JOINT_QUAT, # useFixedBase=0) - ground = pb.loadURDF(cwd + "/robot_model/ground/plane.urdf", - useFixedBase=1) + ground = pb.loadURDF(cwd + "/robot_model/ground/plane.urdf", useFixedBase=1) pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) # TODO:modify this function without dictionary container @@ -671,18 +606,17 @@ def signal_handler(signal, frame): active_joint_idx_list = [] # default robot kinematics information - base_com_pos, base_com_quat = pb.getBasePositionAndOrientation( - draco_humanoid) + base_com_pos, base_com_quat = pb.getBasePositionAndOrientation(draco_humanoid) rot_world_basecom = util.quat_to_rot(np.array(base_com_quat)) - rot_world_basejoint = util.quat_to_rot( - np.array(Config.INITIAL_BASE_JOINT_QUAT)) + rot_world_basejoint = util.quat_to_rot(np.array(Config.INITIAL_BASE_JOINT_QUAT)) pos_basejoint_to_basecom = np.dot( rot_world_basejoint.transpose(), base_com_pos - np.array(Config.INITIAL_BASE_JOINT_POS), ) - rot_basejoint_to_basecom = np.dot(rot_world_basejoint.transpose(), - rot_world_basecom) + rot_basejoint_to_basecom = np.dot( + rot_world_basejoint.transpose(), rot_world_basecom + ) # Run Simulation dt = Config.CONTROLLER_DT @@ -710,28 +644,31 @@ def signal_handler(signal, frame): # Moving Camera Setting ############################################################ base_pos, base_ori = pb.getBasePositionAndOrientation(draco_humanoid) - pb.resetDebugVisualizerCamera(cameraDistance=1.5, - cameraYaw=120, - cameraPitch=-30, - cameraTargetPosition=base_pos + - np.array([0.5, 0.3, -base_pos[2] + 1])) + pb.resetDebugVisualizerCamera( + cameraDistance=1.5, + cameraYaw=120, + cameraPitch=-30, + cameraTargetPosition=base_pos + np.array([0.5, 0.3, -base_pos[2] + 1]), + ) ############################################################################### # Debugging Purpose ############################################################################## ##debugging state estimator by calculating groundtruth basejoint states - base_com_pos, base_com_quat = pb.getBasePositionAndOrientation( - draco_humanoid) + base_com_pos, base_com_quat = pb.getBasePositionAndOrientation(draco_humanoid) rot_world_basecom = util.quat_to_rot(base_com_quat) - rot_world_basejoint = np.dot(rot_world_basecom, - rot_basejoint_to_basecom.transpose()) - base_joint_pos = base_com_pos - np.dot(rot_world_basejoint, - pos_basejoint_to_basecom) + rot_world_basejoint = np.dot( + rot_world_basecom, rot_basejoint_to_basecom.transpose() + ) + base_joint_pos = base_com_pos - np.dot( + rot_world_basejoint, pos_basejoint_to_basecom + ) base_joint_quat = util.rot_to_quat(rot_world_basejoint) base_com_lin_vel, base_com_ang_vel = pb.getBaseVelocity(draco_humanoid) - trans_joint_com = liegroup.RpToTrans(rot_basejoint_to_basecom, - pos_basejoint_to_basecom) + trans_joint_com = liegroup.RpToTrans( + rot_basejoint_to_basecom, pos_basejoint_to_basecom + ) adjoint_joint_com = liegroup.Adjoint(trans_joint_com) twist_basecom_in_world = np.zeros(6) twist_basecom_in_world[0:3] = base_com_ang_vel @@ -739,15 +676,16 @@ def signal_handler(signal, frame): augrot_basecom_world = np.zeros((6, 6)) augrot_basecom_world[0:3, 0:3] = rot_world_basecom.transpose() augrot_basecom_world[3:6, 3:6] = rot_world_basecom.transpose() - twist_basecom_in_basecom = np.dot(augrot_basecom_world, - twist_basecom_in_world) - twist_basejoint_in_basejoint = np.dot(adjoint_joint_com, - twist_basecom_in_basecom) + twist_basecom_in_basecom = np.dot(augrot_basecom_world, twist_basecom_in_world) + twist_basejoint_in_basejoint = np.dot( + adjoint_joint_com, twist_basecom_in_basecom + ) augrot_world_basejoint = np.zeros((6, 6)) augrot_world_basejoint[0:3, 0:3] = rot_world_basejoint augrot_world_basejoint[3:6, 3:6] = rot_world_basejoint - twist_basejoint_in_world = np.dot(augrot_world_basejoint, - twist_basejoint_in_basejoint) + twist_basejoint_in_world = np.dot( + augrot_world_basejoint, twist_basejoint_in_basejoint + ) base_joint_ang_vel = twist_basejoint_in_world[0:3] base_joint_lin_vel = twist_basejoint_in_world[3:6] @@ -790,8 +728,9 @@ def signal_handler(signal, frame): elif pybullet_util.is_key_triggered(keys, "p"): pos = [0.0, 0.0, 0.0] force = [0.0, pb.readUserDebugParameter(perturb), 0.0] - pb.applyExternalForce(draco_humanoid, DracoLinkIdx.torso_com_link, - force, pos, pb.WORLD_FRAME) + pb.applyExternalForce( + draco_humanoid, DracoLinkIdx.torso_com_link, force, pos, pb.WORLD_FRAME + ) print(f"=================force: {force}=================") # get sensor data @@ -821,8 +760,7 @@ def signal_handler(signal, frame): if Config.MEASURE_COMPUTATION_TIME: timer.tic() - rpc_draco_interface.GetCommand(rpc_draco_sensor_data, - rpc_draco_command) + rpc_draco_interface.GetCommand(rpc_draco_sensor_data, rpc_draco_command) if Config.MEASURE_COMPUTATION_TIME: comp_time = timer.tocvalue() @@ -855,7 +793,8 @@ def signal_handler(signal, frame): # lfoot_pos = pybullet_util.get_link_iso(draco_humanoid, # save current torso velocity for next iteration previous_torso_velocity = pybullet_util.get_link_vel( - draco_humanoid, link_id_dict["torso_imu"])[3:6] + draco_humanoid, link_id_dict["torso_imu"] + )[3:6] # DracoLinkIdx.l_foot_contact)[0:3, 3] # rfoot_pos = pybullet_util.get_link_iso(draco_humanoid, @@ -874,8 +813,9 @@ def signal_handler(signal, frame): if (Config.VIDEO_RECORD) and (count % Config.RECORD_FREQ == 0): camera_data = pb.getDebugVisualizerCamera() frame = pybullet_util.get_camera_image_from_debug_camera( - camera_data, Config.RENDER_WIDTH, Config.RENDER_HEIGHT) - filename = video_dir + '/step%06d.jpg' % jpg_count + camera_data, Config.RENDER_WIDTH, Config.RENDER_HEIGHT + ) + filename = video_dir + "/step%06d.jpg" % jpg_count cv2.imwrite(filename, frame) jpg_count += 1 diff --git a/simulator/pybullet/draco_manipulation_main.py b/simulator/pybullet/draco_manipulation_main.py index 02549975..2d1a3966 100644 --- a/simulator/pybullet/draco_manipulation_main.py +++ b/simulator/pybullet/draco_manipulation_main.py @@ -45,154 +45,111 @@ def get_sensor_data_from_pybullet(robot): joint_pos, joint_vel = np.zeros(27), np.zeros(27) imu_frame_quat = np.array( - pb.getLinkState(robot, DracoManipulationLinkIdx.torso_imu, 1, 1)[1]) + pb.getLinkState(robot, DracoManipulationLinkIdx.torso_imu, 1, 1)[1] + ) # LF - joint_pos[0] = pb.getJointState(robot, - DracoManipulationJointIdx.l_hip_ie)[0] - joint_pos[1] = pb.getJointState(robot, - DracoManipulationJointIdx.l_hip_aa)[0] - joint_pos[2] = pb.getJointState(robot, - DracoManipulationJointIdx.l_hip_fe)[0] - joint_pos[3] = pb.getJointState(robot, - DracoManipulationJointIdx.l_knee_fe_jp)[0] - joint_pos[4] = pb.getJointState(robot, - DracoManipulationJointIdx.l_knee_fe_jd)[0] - joint_pos[5] = pb.getJointState(robot, - DracoManipulationJointIdx.l_ankle_fe)[0] - joint_pos[6] = pb.getJointState(robot, - DracoManipulationJointIdx.l_ankle_ie)[0] + joint_pos[0] = pb.getJointState(robot, DracoManipulationJointIdx.l_hip_ie)[0] + joint_pos[1] = pb.getJointState(robot, DracoManipulationJointIdx.l_hip_aa)[0] + joint_pos[2] = pb.getJointState(robot, DracoManipulationJointIdx.l_hip_fe)[0] + joint_pos[3] = pb.getJointState(robot, DracoManipulationJointIdx.l_knee_fe_jp)[0] + joint_pos[4] = pb.getJointState(robot, DracoManipulationJointIdx.l_knee_fe_jd)[0] + joint_pos[5] = pb.getJointState(robot, DracoManipulationJointIdx.l_ankle_fe)[0] + joint_pos[6] = pb.getJointState(robot, DracoManipulationJointIdx.l_ankle_ie)[0] # LH - joint_pos[7] = pb.getJointState(robot, - DracoManipulationJointIdx.l_shoulder_fe)[0] - joint_pos[8] = pb.getJointState(robot, - DracoManipulationJointIdx.l_shoulder_aa)[0] - joint_pos[9] = pb.getJointState(robot, - DracoManipulationJointIdx.l_shoulder_ie)[0] - joint_pos[10] = pb.getJointState(robot, - DracoManipulationJointIdx.l_elbow_fe)[0] - joint_pos[11] = pb.getJointState(robot, - DracoManipulationJointIdx.l_wrist_ps)[0] - joint_pos[12] = pb.getJointState( - robot, DracoManipulationJointIdx.l_wrist_pitch)[0] + joint_pos[7] = pb.getJointState(robot, DracoManipulationJointIdx.l_shoulder_fe)[0] + joint_pos[8] = pb.getJointState(robot, DracoManipulationJointIdx.l_shoulder_aa)[0] + joint_pos[9] = pb.getJointState(robot, DracoManipulationJointIdx.l_shoulder_ie)[0] + joint_pos[10] = pb.getJointState(robot, DracoManipulationJointIdx.l_elbow_fe)[0] + joint_pos[11] = pb.getJointState(robot, DracoManipulationJointIdx.l_wrist_ps)[0] + joint_pos[12] = pb.getJointState(robot, DracoManipulationJointIdx.l_wrist_pitch)[0] # neck - joint_pos[13] = pb.getJointState(robot, - DracoManipulationJointIdx.neck_pitch)[0] + joint_pos[13] = pb.getJointState(robot, DracoManipulationJointIdx.neck_pitch)[0] # RF - joint_pos[14] = pb.getJointState(robot, - DracoManipulationJointIdx.r_hip_ie)[0] - joint_pos[15] = pb.getJointState(robot, - DracoManipulationJointIdx.r_hip_aa)[0] - joint_pos[16] = pb.getJointState(robot, - DracoManipulationJointIdx.r_hip_fe)[0] - joint_pos[17] = pb.getJointState(robot, - DracoManipulationJointIdx.r_knee_fe_jp)[0] - joint_pos[18] = pb.getJointState(robot, - DracoManipulationJointIdx.r_knee_fe_jd)[0] - joint_pos[19] = pb.getJointState(robot, - DracoManipulationJointIdx.r_ankle_fe)[0] - joint_pos[20] = pb.getJointState(robot, - DracoManipulationJointIdx.r_ankle_ie)[0] + joint_pos[14] = pb.getJointState(robot, DracoManipulationJointIdx.r_hip_ie)[0] + joint_pos[15] = pb.getJointState(robot, DracoManipulationJointIdx.r_hip_aa)[0] + joint_pos[16] = pb.getJointState(robot, DracoManipulationJointIdx.r_hip_fe)[0] + joint_pos[17] = pb.getJointState(robot, DracoManipulationJointIdx.r_knee_fe_jp)[0] + joint_pos[18] = pb.getJointState(robot, DracoManipulationJointIdx.r_knee_fe_jd)[0] + joint_pos[19] = pb.getJointState(robot, DracoManipulationJointIdx.r_ankle_fe)[0] + joint_pos[20] = pb.getJointState(robot, DracoManipulationJointIdx.r_ankle_ie)[0] # RH - joint_pos[21] = pb.getJointState( - robot, DracoManipulationJointIdx.r_shoulder_fe)[0] - joint_pos[22] = pb.getJointState( - robot, DracoManipulationJointIdx.r_shoulder_aa)[0] - joint_pos[23] = pb.getJointState( - robot, DracoManipulationJointIdx.r_shoulder_ie)[0] - joint_pos[24] = pb.getJointState(robot, - DracoManipulationJointIdx.r_elbow_fe)[0] - joint_pos[25] = pb.getJointState(robot, - DracoManipulationJointIdx.r_wrist_ps)[0] - joint_pos[26] = pb.getJointState( - robot, DracoManipulationJointIdx.r_wrist_pitch)[0] + joint_pos[21] = pb.getJointState(robot, DracoManipulationJointIdx.r_shoulder_fe)[0] + joint_pos[22] = pb.getJointState(robot, DracoManipulationJointIdx.r_shoulder_aa)[0] + joint_pos[23] = pb.getJointState(robot, DracoManipulationJointIdx.r_shoulder_ie)[0] + joint_pos[24] = pb.getJointState(robot, DracoManipulationJointIdx.r_elbow_fe)[0] + joint_pos[25] = pb.getJointState(robot, DracoManipulationJointIdx.r_wrist_ps)[0] + joint_pos[26] = pb.getJointState(robot, DracoManipulationJointIdx.r_wrist_pitch)[0] imu_ang_vel = np.array( - pb.getLinkState(robot, DracoManipulationLinkIdx.torso_imu, 1, 1)[7]) + pb.getLinkState(robot, DracoManipulationLinkIdx.torso_imu, 1, 1)[7] + ) imu_dvel = pybullet_util.simulate_dVel_data( - robot, DracoManipulationLinkIdx.torso_imu, previous_torso_velocity) + robot, DracoManipulationLinkIdx.torso_imu, previous_torso_velocity + ) # LF - joint_vel[0] = pb.getJointState(robot, - DracoManipulationJointIdx.l_hip_ie)[1] - joint_vel[1] = pb.getJointState(robot, - DracoManipulationJointIdx.l_hip_aa)[1] - joint_vel[2] = pb.getJointState(robot, - DracoManipulationJointIdx.l_hip_fe)[1] - joint_vel[3] = pb.getJointState(robot, - DracoManipulationJointIdx.l_knee_fe_jp)[1] - joint_vel[4] = pb.getJointState(robot, - DracoManipulationJointIdx.l_knee_fe_jd)[1] - joint_vel[5] = pb.getJointState(robot, - DracoManipulationJointIdx.l_ankle_fe)[1] - joint_vel[6] = pb.getJointState(robot, - DracoManipulationJointIdx.l_ankle_ie)[1] + joint_vel[0] = pb.getJointState(robot, DracoManipulationJointIdx.l_hip_ie)[1] + joint_vel[1] = pb.getJointState(robot, DracoManipulationJointIdx.l_hip_aa)[1] + joint_vel[2] = pb.getJointState(robot, DracoManipulationJointIdx.l_hip_fe)[1] + joint_vel[3] = pb.getJointState(robot, DracoManipulationJointIdx.l_knee_fe_jp)[1] + joint_vel[4] = pb.getJointState(robot, DracoManipulationJointIdx.l_knee_fe_jd)[1] + joint_vel[5] = pb.getJointState(robot, DracoManipulationJointIdx.l_ankle_fe)[1] + joint_vel[6] = pb.getJointState(robot, DracoManipulationJointIdx.l_ankle_ie)[1] # LH - joint_vel[7] = pb.getJointState(robot, - DracoManipulationJointIdx.l_shoulder_fe)[1] - joint_vel[8] = pb.getJointState(robot, - DracoManipulationJointIdx.l_shoulder_aa)[1] - joint_vel[9] = pb.getJointState(robot, - DracoManipulationJointIdx.l_shoulder_ie)[1] - joint_vel[10] = pb.getJointState(robot, - DracoManipulationJointIdx.l_elbow_fe)[1] - joint_vel[11] = pb.getJointState(robot, - DracoManipulationJointIdx.l_wrist_ps)[1] - joint_vel[12] = pb.getJointState( - robot, DracoManipulationJointIdx.l_wrist_pitch)[1] + joint_vel[7] = pb.getJointState(robot, DracoManipulationJointIdx.l_shoulder_fe)[1] + joint_vel[8] = pb.getJointState(robot, DracoManipulationJointIdx.l_shoulder_aa)[1] + joint_vel[9] = pb.getJointState(robot, DracoManipulationJointIdx.l_shoulder_ie)[1] + joint_vel[10] = pb.getJointState(robot, DracoManipulationJointIdx.l_elbow_fe)[1] + joint_vel[11] = pb.getJointState(robot, DracoManipulationJointIdx.l_wrist_ps)[1] + joint_vel[12] = pb.getJointState(robot, DracoManipulationJointIdx.l_wrist_pitch)[1] # neck - joint_vel[13] = pb.getJointState(robot, - DracoManipulationJointIdx.neck_pitch)[1] + joint_vel[13] = pb.getJointState(robot, DracoManipulationJointIdx.neck_pitch)[1] # RF - joint_vel[14] = pb.getJointState(robot, - DracoManipulationJointIdx.r_hip_ie)[1] - joint_vel[15] = pb.getJointState(robot, - DracoManipulationJointIdx.r_hip_aa)[1] - joint_vel[16] = pb.getJointState(robot, - DracoManipulationJointIdx.r_hip_fe)[1] - joint_vel[17] = pb.getJointState(robot, - DracoManipulationJointIdx.r_knee_fe_jp)[1] - joint_vel[18] = pb.getJointState(robot, - DracoManipulationJointIdx.r_knee_fe_jd)[1] - joint_vel[19] = pb.getJointState(robot, - DracoManipulationJointIdx.r_ankle_fe)[1] - joint_vel[20] = pb.getJointState(robot, - DracoManipulationJointIdx.r_ankle_ie)[1] + joint_vel[14] = pb.getJointState(robot, DracoManipulationJointIdx.r_hip_ie)[1] + joint_vel[15] = pb.getJointState(robot, DracoManipulationJointIdx.r_hip_aa)[1] + joint_vel[16] = pb.getJointState(robot, DracoManipulationJointIdx.r_hip_fe)[1] + joint_vel[17] = pb.getJointState(robot, DracoManipulationJointIdx.r_knee_fe_jp)[1] + joint_vel[18] = pb.getJointState(robot, DracoManipulationJointIdx.r_knee_fe_jd)[1] + joint_vel[19] = pb.getJointState(robot, DracoManipulationJointIdx.r_ankle_fe)[1] + joint_vel[20] = pb.getJointState(robot, DracoManipulationJointIdx.r_ankle_ie)[1] # RH - joint_vel[21] = pb.getJointState( - robot, DracoManipulationJointIdx.r_shoulder_fe)[1] - joint_vel[22] = pb.getJointState( - robot, DracoManipulationJointIdx.r_shoulder_aa)[1] - joint_vel[23] = pb.getJointState( - robot, DracoManipulationJointIdx.r_shoulder_ie)[1] - joint_vel[24] = pb.getJointState(robot, - DracoManipulationJointIdx.r_elbow_fe)[1] - joint_vel[25] = pb.getJointState(robot, - DracoManipulationJointIdx.r_wrist_ps)[1] - joint_vel[26] = pb.getJointState( - robot, DracoManipulationJointIdx.r_wrist_pitch)[1] + joint_vel[21] = pb.getJointState(robot, DracoManipulationJointIdx.r_shoulder_fe)[1] + joint_vel[22] = pb.getJointState(robot, DracoManipulationJointIdx.r_shoulder_aa)[1] + joint_vel[23] = pb.getJointState(robot, DracoManipulationJointIdx.r_shoulder_ie)[1] + joint_vel[24] = pb.getJointState(robot, DracoManipulationJointIdx.r_elbow_fe)[1] + joint_vel[25] = pb.getJointState(robot, DracoManipulationJointIdx.r_wrist_ps)[1] + joint_vel[26] = pb.getJointState(robot, DracoManipulationJointIdx.r_wrist_pitch)[1] # normal force measured on each foot _l_normal_force = 0 contacts = pb.getContactPoints( - bodyA=robot, linkIndexA=DracoManipulationLinkIdx.l_ankle_ie_link) + bodyA=robot, linkIndexA=DracoManipulationLinkIdx.l_ankle_ie_link + ) for contact in contacts: # add z-component on all points of contact _l_normal_force += contact[9] _r_normal_force = 0 contacts = pb.getContactPoints( - bodyA=robot, linkIndexA=DracoManipulationLinkIdx.r_ankle_ie_link) + bodyA=robot, linkIndexA=DracoManipulationLinkIdx.r_ankle_ie_link + ) for contact in contacts: # add z-component on all points of contact _r_normal_force += contact[9] - b_lf_contact = (True if pb.getLinkState( - robot, DracoManipulationLinkIdx.l_foot_contact, 1, 1)[0][2] <= 0.05 - else False) - b_rf_contact = (True if pb.getLinkState( - robot, DracoManipulationLinkIdx.r_foot_contact, 1, 1)[0][2] <= 0.05 - else False) + b_lf_contact = ( + True + if pb.getLinkState(robot, DracoManipulationLinkIdx.l_foot_contact, 1, 1)[0][2] + <= 0.05 + else False + ) + b_rf_contact = ( + True + if pb.getLinkState(robot, DracoManipulationLinkIdx.r_foot_contact, 1, 1)[0][2] + <= 0.05 + else False + ) return ( imu_frame_quat, imu_ang_vel, @@ -211,18 +168,15 @@ def apply_control_input_to_pybullet(robot, command): mode = pb.TORQUE_CONTROL # LF - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.l_hip_ie, - controlMode=mode, - force=command[0]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.l_hip_aa, - controlMode=mode, - force=command[1]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.l_hip_fe, - controlMode=mode, - force=command[2]) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.l_hip_ie, controlMode=mode, force=command[0] + ) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.l_hip_aa, controlMode=mode, force=command[1] + ) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.l_hip_fe, controlMode=mode, force=command[2] + ) # pb.setJointMotorControl2(robot, DracoManipulationJointIdx.l_knee_fe_jp, controlMode=mode, force=command[3]) pb.setJointMotorControl2( robot, @@ -230,14 +184,12 @@ def apply_control_input_to_pybullet(robot, command): controlMode=mode, force=command[4], ) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.l_ankle_fe, - controlMode=mode, - force=command[5]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.l_ankle_ie, - controlMode=mode, - force=command[6]) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.l_ankle_fe, controlMode=mode, force=command[5] + ) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.l_ankle_ie, controlMode=mode, force=command[6] + ) # LH pb.setJointMotorControl2( @@ -258,14 +210,12 @@ def apply_control_input_to_pybullet(robot, command): controlMode=mode, force=command[9], ) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.l_elbow_fe, - controlMode=mode, - force=command[10]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.l_wrist_ps, - controlMode=mode, - force=command[11]) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.l_elbow_fe, controlMode=mode, force=command[10] + ) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.l_wrist_ps, controlMode=mode, force=command[11] + ) pb.setJointMotorControl2( robot, DracoManipulationJointIdx.l_wrist_pitch, @@ -274,24 +224,20 @@ def apply_control_input_to_pybullet(robot, command): ) # neck - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.neck_pitch, - controlMode=mode, - force=command[13]) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.neck_pitch, controlMode=mode, force=command[13] + ) # RF - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.r_hip_ie, - controlMode=mode, - force=command[14]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.r_hip_aa, - controlMode=mode, - force=command[15]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.r_hip_fe, - controlMode=mode, - force=command[16]) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.r_hip_ie, controlMode=mode, force=command[14] + ) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.r_hip_aa, controlMode=mode, force=command[15] + ) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.r_hip_fe, controlMode=mode, force=command[16] + ) # pb.setJointMotorControl2(robot, DracoManipulationJointIdx.r_knee_fe_jd, controlMode=mode, force=command[17]) pb.setJointMotorControl2( robot, @@ -299,14 +245,12 @@ def apply_control_input_to_pybullet(robot, command): controlMode=mode, force=command[18], ) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.r_ankle_fe, - controlMode=mode, - force=command[19]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.r_ankle_ie, - controlMode=mode, - force=command[20]) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.r_ankle_fe, controlMode=mode, force=command[19] + ) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.r_ankle_ie, controlMode=mode, force=command[20] + ) # RH pb.setJointMotorControl2( @@ -327,14 +271,12 @@ def apply_control_input_to_pybullet(robot, command): controlMode=mode, force=command[23], ) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.r_elbow_fe, - controlMode=mode, - force=command[24]) - pb.setJointMotorControl2(robot, - DracoManipulationJointIdx.r_wrist_ps, - controlMode=mode, - force=command[25]) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.r_elbow_fe, controlMode=mode, force=command[24] + ) + pb.setJointMotorControl2( + robot, DracoManipulationJointIdx.r_wrist_ps, controlMode=mode, force=command[25] + ) pb.setJointMotorControl2( robot, DracoManipulationJointIdx.r_wrist_pitch, @@ -345,42 +287,34 @@ def apply_control_input_to_pybullet(robot, command): def set_init_config_pybullet_robot(robot): # Upperbody - pb.resetJointState(robot, DracoManipulationJointIdx.l_shoulder_aa, - np.pi / 6, 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.l_elbow_fe, -np.pi / 2, - 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.r_shoulder_aa, - -np.pi / 6, 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.r_elbow_fe, -np.pi / 2, - 0.0) + pb.resetJointState(robot, DracoManipulationJointIdx.l_shoulder_aa, np.pi / 6, 0.0) + pb.resetJointState(robot, DracoManipulationJointIdx.l_elbow_fe, -np.pi / 2, 0.0) + pb.resetJointState(robot, DracoManipulationJointIdx.r_shoulder_aa, -np.pi / 6, 0.0) + pb.resetJointState(robot, DracoManipulationJointIdx.r_elbow_fe, -np.pi / 2, 0.0) # Lowerbody hip_yaw_angle = 0 - pb.resetJointState(robot, DracoManipulationJointIdx.l_hip_aa, - np.radians(hip_yaw_angle), 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.l_hip_fe, -np.pi / 4, - 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.l_knee_fe_jp, - np.pi / 4, 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.l_knee_fe_jd, - np.pi / 4, 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.l_ankle_fe, -np.pi / 4, - 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.l_ankle_ie, - np.radians(-hip_yaw_angle), 0.0) - - pb.resetJointState(robot, DracoManipulationJointIdx.r_hip_aa, - np.radians(-hip_yaw_angle), 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.r_hip_fe, -np.pi / 4, - 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.r_knee_fe_jp, - np.pi / 4, 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.r_knee_fe_jd, - np.pi / 4, 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.r_ankle_fe, -np.pi / 4, - 0.0) - pb.resetJointState(robot, DracoManipulationJointIdx.r_ankle_ie, - np.radians(hip_yaw_angle), 0.0) + pb.resetJointState( + robot, DracoManipulationJointIdx.l_hip_aa, np.radians(hip_yaw_angle), 0.0 + ) + pb.resetJointState(robot, DracoManipulationJointIdx.l_hip_fe, -np.pi / 4, 0.0) + pb.resetJointState(robot, DracoManipulationJointIdx.l_knee_fe_jp, np.pi / 4, 0.0) + pb.resetJointState(robot, DracoManipulationJointIdx.l_knee_fe_jd, np.pi / 4, 0.0) + pb.resetJointState(robot, DracoManipulationJointIdx.l_ankle_fe, -np.pi / 4, 0.0) + pb.resetJointState( + robot, DracoManipulationJointIdx.l_ankle_ie, np.radians(-hip_yaw_angle), 0.0 + ) + + pb.resetJointState( + robot, DracoManipulationJointIdx.r_hip_aa, np.radians(-hip_yaw_angle), 0.0 + ) + pb.resetJointState(robot, DracoManipulationJointIdx.r_hip_fe, -np.pi / 4, 0.0) + pb.resetJointState(robot, DracoManipulationJointIdx.r_knee_fe_jp, np.pi / 4, 0.0) + pb.resetJointState(robot, DracoManipulationJointIdx.r_knee_fe_jd, np.pi / 4, 0.0) + pb.resetJointState(robot, DracoManipulationJointIdx.r_ankle_fe, -np.pi / 4, 0.0) + pb.resetJointState( + robot, DracoManipulationJointIdx.r_ankle_ie, np.radians(hip_yaw_angle), 0.0 + ) def signal_handler(signal, frame): @@ -390,9 +324,9 @@ def signal_handler(signal, frame): print("========================================================") print('saving list of compuation time in "compuation_time.txt"') print("========================================================") - np.savetxt("computation_time.txt", - np.array([compuation_cal_list]), - delimiter=",") + np.savetxt( + "computation_time.txt", np.array([compuation_cal_list]), delimiter="," + ) if Config.VIDEO_RECORD: print("========================================================") @@ -418,8 +352,9 @@ def signal_handler(signal, frame): cameraTargetPosition=[0, 0, 0.5], ) ## sim physics setting - pb.setPhysicsEngineParameter(fixedTimeStep=Config.CONTROLLER_DT, - numSubSteps=Config.N_SUBSTEP) + pb.setPhysicsEngineParameter( + fixedTimeStep=Config.CONTROLLER_DT, numSubSteps=Config.N_SUBSTEP + ) pb.setGravity(0, 0, -9.81) ## robot spawn & initial kinematics and dynamics setting @@ -435,8 +370,7 @@ def signal_handler(signal, frame): useFixedBase=0, ) - ground = pb.loadURDF(cwd + "/robot_model/ground/plane.urdf", - useFixedBase=1) + ground = pb.loadURDF(cwd + "/robot_model/ground/plane.urdf", useFixedBase=1) xOffset = 0.7 pb.loadURDF( @@ -522,18 +456,17 @@ def signal_handler(signal, frame): active_jointidx_list = [] # for setjointmotorcontrolarray # default robot kinematics information - base_com_pos, base_com_quat = pb.getBasePositionAndOrientation( - draco_humanoid) + base_com_pos, base_com_quat = pb.getBasePositionAndOrientation(draco_humanoid) rot_world_basecom = util.quat_to_rot(np.array(base_com_quat)) - rot_world_basejoint = util.quat_to_rot( - np.array(Config.INITIAL_BASE_JOINT_QUAT)) + rot_world_basejoint = util.quat_to_rot(np.array(Config.INITIAL_BASE_JOINT_QUAT)) pos_basejoint_to_basecom = np.dot( rot_world_basejoint.transpose(), base_com_pos - np.array(Config.INITIAL_BASE_JOINT_POS), ) - rot_basejoint_to_basecom = np.dot(rot_world_basejoint.transpose(), - rot_world_basecom) + rot_basejoint_to_basecom = np.dot( + rot_world_basejoint.transpose(), rot_world_basecom + ) # Run Simulation dt = Config.CONTROLLER_DT @@ -580,18 +513,20 @@ def signal_handler(signal, frame): # Debugging Purpose ############################################################################## ##debugging state estimator by calculating groundtruth basejoint states - base_com_pos, base_com_quat = pb.getBasePositionAndOrientation( - draco_humanoid) + base_com_pos, base_com_quat = pb.getBasePositionAndOrientation(draco_humanoid) rot_world_basecom = util.quat_to_rot(base_com_quat) - rot_world_basejoint = np.dot(rot_world_basecom, - rot_basejoint_to_basecom.transpose()) - base_joint_pos = base_com_pos - np.dot(rot_world_basejoint, - pos_basejoint_to_basecom) + rot_world_basejoint = np.dot( + rot_world_basecom, rot_basejoint_to_basecom.transpose() + ) + base_joint_pos = base_com_pos - np.dot( + rot_world_basejoint, pos_basejoint_to_basecom + ) base_joint_quat = util.rot_to_quat(rot_world_basejoint) base_com_lin_vel, base_com_ang_vel = pb.getBaseVelocity(draco_humanoid) - trans_joint_com = liegroup.RpToTrans(rot_basejoint_to_basecom, - pos_basejoint_to_basecom) + trans_joint_com = liegroup.RpToTrans( + rot_basejoint_to_basecom, pos_basejoint_to_basecom + ) adjoint_joint_com = liegroup.Adjoint(trans_joint_com) twist_basecom_in_world = np.zeros(6) twist_basecom_in_world[0:3] = base_com_ang_vel @@ -599,15 +534,16 @@ def signal_handler(signal, frame): augrot_basecom_world = np.zeros((6, 6)) augrot_basecom_world[0:3, 0:3] = rot_world_basecom.transpose() augrot_basecom_world[3:6, 3:6] = rot_world_basecom.transpose() - twist_basecom_in_basecom = np.dot(augrot_basecom_world, - twist_basecom_in_world) - twist_basejoint_in_basejoint = np.dot(adjoint_joint_com, - twist_basecom_in_basecom) + twist_basecom_in_basecom = np.dot(augrot_basecom_world, twist_basecom_in_world) + twist_basejoint_in_basejoint = np.dot( + adjoint_joint_com, twist_basecom_in_basecom + ) augrot_world_basejoint = np.zeros((6, 6)) augrot_world_basejoint[0:3, 0:3] = rot_world_basejoint augrot_world_basejoint[3:6, 3:6] = rot_world_basejoint - twist_basejoint_in_world = np.dot(augrot_world_basejoint, - twist_basejoint_in_basejoint) + twist_basejoint_in_world = np.dot( + augrot_world_basejoint, twist_basejoint_in_basejoint + ) base_joint_ang_vel = twist_basejoint_in_world[0:3] base_joint_lin_vel = twist_basejoint_in_world[3:6] @@ -661,12 +597,13 @@ def signal_handler(signal, frame): l_normal_force = pybullet_util.simulate_contact_sensor(l_normal_force) r_normal_force = pybullet_util.simulate_contact_sensor(r_normal_force) imu_dvel = pybullet_util.add_sensor_noise(imu_dvel, imu_dvel_bias) - imu_ang_vel = pybullet_util.add_sensor_noise(imu_ang_vel, - imu_ang_vel_noise) + imu_ang_vel = pybullet_util.add_sensor_noise(imu_ang_vel, imu_ang_vel_noise) l_normal_force = pybullet_util.add_sensor_noise( - l_normal_force, l_normal_volt_noise) + l_normal_force, l_normal_volt_noise + ) r_normal_force = pybullet_util.add_sensor_noise( - r_normal_force, r_normal_volt_noise) + r_normal_force, r_normal_volt_noise + ) # copy sensor data to rpc sensor data class rpc_draco_sensor_data.imu_frame_quat_ = imu_frame_quat @@ -686,8 +623,7 @@ def signal_handler(signal, frame): if Config.MEASURE_COMPUTATION_TIME: timer.tic() - rpc_draco_interface.GetCommand(rpc_draco_sensor_data, - rpc_draco_command) + rpc_draco_interface.GetCommand(rpc_draco_sensor_data, rpc_draco_command) if Config.MEASURE_COMPUTATION_TIME: comp_time = timer.tocvalue() @@ -704,7 +640,8 @@ def signal_handler(signal, frame): # lfoot_pos = pybullet_util.get_link_iso(draco_humanoid, # save current torso velocity for next iteration previous_torso_velocity = pybullet_util.get_link_vel( - draco_humanoid, link_id_dict["torso_imu"])[3:6] + draco_humanoid, link_id_dict["torso_imu"] + )[3:6] # DracoManipulationLinkIdx.l_foot_contact)[0:3, 3] # rfoot_pos = pybullet_util.get_link_iso(draco_humanoid, @@ -723,7 +660,8 @@ def signal_handler(signal, frame): if (Config.VIDEO_RECORD) and (count % Config.RECORD_FREQ == 0): camera_data = pb.getDebugVisualizerCamera() frame = pybullet_util.get_camera_image_from_debug_camera( - camera_data, Config.RENDER_WIDTH, Config.RENDER_HEIGHT) + camera_data, Config.RENDER_WIDTH, Config.RENDER_HEIGHT + ) filename = video_dir + "/step%06d.jpg" % jpg_count cv2.imwrite(filename, frame) jpg_count += 1 diff --git a/simulator/pybullet/go2_main.py b/simulator/pybullet/go2_main.py index d320d0d7..dd3cd1f9 100644 --- a/simulator/pybullet/go2_main.py +++ b/simulator/pybullet/go2_main.py @@ -45,8 +45,9 @@ def get_sensor_data_from_pybullet(robot): imu_ang_vel = np.array(pb.getLinkState(robot, Go2LinkIdx.imu, 1, 1)[7]) - imu_dvel = pybullet_util.simulate_dVel_data(robot, Go2LinkIdx.imu, - previous_torso_velocity) + imu_dvel = pybullet_util.simulate_dVel_data( + robot, Go2LinkIdx.imu, previous_torso_velocity + ) # FL joint_vel[0] = pb.getJointState(robot, Go2JointIdx.FL_hip_joint)[1] @@ -90,98 +91,101 @@ def get_sensor_data_from_pybullet(robot): # add z-component on all points of contact RR_normal_force += contact[9] - b_FL_foot_contact = (True if pb.getLinkState(robot, Go2LinkIdx.FL_foot, 1, - 1)[0][2] <= 0.05 else False) - b_FR_foot_contact = (True if pb.getLinkState(robot, Go2LinkIdx.FR_foot, 1, - 1)[0][2] <= 0.05 else False) - b_RL_foot_contact = (True if pb.getLinkState(robot, Go2LinkIdx.RL_foot, 1, - 1)[0][2] <= 0.05 else False) - b_RR_foot_contact = (True if pb.getLinkState(robot, Go2LinkIdx.RR_foot, 1, - 1)[0][2] <= 0.05 else False) - return (imu_frame_quat, imu_ang_vel, imu_dvel, joint_pos, joint_vel, - b_FL_foot_contact, b_FR_foot_contact, b_RL_foot_contact, - b_RR_foot_contact, FL_normal_force, FR_normal_force, - RL_normal_force, RR_normal_force) + b_FL_foot_contact = ( + True + if pb.getLinkState(robot, Go2LinkIdx.FL_foot, 1, 1)[0][2] <= 0.05 + else False + ) + b_FR_foot_contact = ( + True + if pb.getLinkState(robot, Go2LinkIdx.FR_foot, 1, 1)[0][2] <= 0.05 + else False + ) + b_RL_foot_contact = ( + True + if pb.getLinkState(robot, Go2LinkIdx.RL_foot, 1, 1)[0][2] <= 0.05 + else False + ) + b_RR_foot_contact = ( + True + if pb.getLinkState(robot, Go2LinkIdx.RR_foot, 1, 1)[0][2] <= 0.05 + else False + ) + return ( + imu_frame_quat, + imu_ang_vel, + imu_dvel, + joint_pos, + joint_vel, + b_FL_foot_contact, + b_FR_foot_contact, + b_RL_foot_contact, + b_RR_foot_contact, + FL_normal_force, + FR_normal_force, + RL_normal_force, + RR_normal_force, + ) def apply_control_input_to_pybullet(robot, command): mode = pb.TORQUE_CONTROL # FL - pb.setJointMotorControl2(robot, - Go2JointIdx.FL_hip_joint, - controlMode=mode, - force=command[0]) - pb.setJointMotorControl2(robot, - Go2JointIdx.FL_thigh_joint, - controlMode=mode, - force=command[1]) - pb.setJointMotorControl2(robot, - Go2JointIdx.FL_calf_joint, - controlMode=mode, - force=command[2]) + pb.setJointMotorControl2( + robot, Go2JointIdx.FL_hip_joint, controlMode=mode, force=command[0] + ) + pb.setJointMotorControl2( + robot, Go2JointIdx.FL_thigh_joint, controlMode=mode, force=command[1] + ) + pb.setJointMotorControl2( + robot, Go2JointIdx.FL_calf_joint, controlMode=mode, force=command[2] + ) # FR - pb.setJointMotorControl2(robot, - Go2JointIdx.FR_hip_joint, - controlMode=mode, - force=command[3]) - pb.setJointMotorControl2(robot, - Go2JointIdx.FR_thigh_joint, - controlMode=mode, - force=command[4]) - pb.setJointMotorControl2(robot, - Go2JointIdx.FR_calf_joint, - controlMode=mode, - force=command[5]) + pb.setJointMotorControl2( + robot, Go2JointIdx.FR_hip_joint, controlMode=mode, force=command[3] + ) + pb.setJointMotorControl2( + robot, Go2JointIdx.FR_thigh_joint, controlMode=mode, force=command[4] + ) + pb.setJointMotorControl2( + robot, Go2JointIdx.FR_calf_joint, controlMode=mode, force=command[5] + ) # RL - pb.setJointMotorControl2(robot, - Go2JointIdx.RL_hip_joint, - controlMode=mode, - force=command[6]) - pb.setJointMotorControl2(robot, - Go2JointIdx.RL_thigh_joint, - controlMode=mode, - force=command[7]) - pb.setJointMotorControl2(robot, - Go2JointIdx.RL_calf_joint, - controlMode=mode, - force=command[8]) + pb.setJointMotorControl2( + robot, Go2JointIdx.RL_hip_joint, controlMode=mode, force=command[6] + ) + pb.setJointMotorControl2( + robot, Go2JointIdx.RL_thigh_joint, controlMode=mode, force=command[7] + ) + pb.setJointMotorControl2( + robot, Go2JointIdx.RL_calf_joint, controlMode=mode, force=command[8] + ) # RR - pb.setJointMotorControl2(robot, - Go2JointIdx.RR_hip_joint, - controlMode=mode, - force=command[9]) - pb.setJointMotorControl2(robot, - Go2JointIdx.RR_thigh_joint, - controlMode=mode, - force=command[10]) - pb.setJointMotorControl2(robot, - Go2JointIdx.RR_calf_joint, - controlMode=mode, - force=command[11]) + pb.setJointMotorControl2( + robot, Go2JointIdx.RR_hip_joint, controlMode=mode, force=command[9] + ) + pb.setJointMotorControl2( + robot, Go2JointIdx.RR_thigh_joint, controlMode=mode, force=command[10] + ) + pb.setJointMotorControl2( + robot, Go2JointIdx.RR_calf_joint, controlMode=mode, force=command[11] + ) def set_init_config_pybullet_robot(robot): knee_angle = 45 - pb.resetJointState(robot, Go2JointIdx.FL_thigh_joint, - np.radians(knee_angle), 0.0) - pb.resetJointState(robot, Go2JointIdx.FL_calf_joint, - np.radians(-knee_angle), 0.0) + pb.resetJointState(robot, Go2JointIdx.FL_thigh_joint, np.radians(knee_angle), 0.0) + pb.resetJointState(robot, Go2JointIdx.FL_calf_joint, np.radians(-knee_angle), 0.0) - pb.resetJointState(robot, Go2JointIdx.FR_thigh_joint, - np.radians(knee_angle), 0.0) - pb.resetJointState(robot, Go2JointIdx.FR_calf_joint, - np.radians(-knee_angle), 0.0) + pb.resetJointState(robot, Go2JointIdx.FR_thigh_joint, np.radians(knee_angle), 0.0) + pb.resetJointState(robot, Go2JointIdx.FR_calf_joint, np.radians(-knee_angle), 0.0) - pb.resetJointState(robot, Go2JointIdx.RL_thigh_joint, - np.radians(knee_angle), 0.0) - pb.resetJointState(robot, Go2JointIdx.RL_calf_joint, - np.radians(-knee_angle), 0.0) + pb.resetJointState(robot, Go2JointIdx.RL_thigh_joint, np.radians(knee_angle), 0.0) + pb.resetJointState(robot, Go2JointIdx.RL_calf_joint, np.radians(-knee_angle), 0.0) - pb.resetJointState(robot, Go2JointIdx.RR_thigh_joint, - np.radians(knee_angle), 0.0) - pb.resetJointState(robot, Go2JointIdx.RR_calf_joint, - np.radians(-knee_angle), 0.0) + pb.resetJointState(robot, Go2JointIdx.RR_thigh_joint, np.radians(knee_angle), 0.0) + pb.resetJointState(robot, Go2JointIdx.RR_calf_joint, np.radians(-knee_angle), 0.0) def signal_handler(signal, frame): @@ -189,9 +193,9 @@ def signal_handler(signal, frame): print("========================================================") print('saving list of compuation time in "compuation_time.txt"') print("========================================================") - np.savetxt("computation_time.txt", - np.array([compuation_cal_list]), - delimiter=",") + np.savetxt( + "computation_time.txt", np.array([compuation_cal_list]), delimiter="," + ) if Config.VIDEO_RECORD: print("========================================================") @@ -217,19 +221,22 @@ def signal_handler(signal, frame): cameraTargetPosition=[0, 0, 0.3], ) ## sim physics setting - pb.setPhysicsEngineParameter(fixedTimeStep=Config.CONTROLLER_DT, - numSubSteps=Config.N_SUBSTEP) + pb.setPhysicsEngineParameter( + fixedTimeStep=Config.CONTROLLER_DT, numSubSteps=Config.N_SUBSTEP + ) pb.setGravity(0, 0, -9.81) ## robot spawn & initial kinematics and dynamics setting pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0) - robot = pb.loadURDF(cwd + "/robot_model/go2/go2_description.urdf", - [0., 0., 0.45], [0, 0, 0, 1], - useFixedBase=False) + robot = pb.loadURDF( + cwd + "/robot_model/go2/go2_description.urdf", + [0.0, 0.0, 0.45], + [0, 0, 0, 1], + useFixedBase=False, + ) - ground = pb.loadURDF(cwd + "/robot_model/ground/plane.urdf", - useFixedBase=1) + ground = pb.loadURDF(cwd + "/robot_model/ground/plane.urdf", useFixedBase=1) pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) ( n_q, @@ -255,15 +262,15 @@ def signal_handler(signal, frame): # default robot kinematics information base_com_pos, base_com_quat = pb.getBasePositionAndOrientation(robot) rot_world_basecom = util.quat_to_rot(np.array(base_com_quat)) - rot_world_basejoint = util.quat_to_rot( - np.array(Config.INITIAL_BASE_JOINT_QUAT)) + rot_world_basejoint = util.quat_to_rot(np.array(Config.INITIAL_BASE_JOINT_QUAT)) pos_basejoint_to_basecom = np.dot( rot_world_basejoint.transpose(), base_com_pos - np.array(Config.INITIAL_BASE_JOINT_POS), ) - rot_basejoint_to_basecom = np.dot(rot_world_basejoint.transpose(), - rot_world_basecom) + rot_basejoint_to_basecom = np.dot( + rot_world_basejoint.transpose(), rot_world_basecom + ) # TODO: pnc interface, sensor_data, command class rpc_go2_interface = go2_interface_py.Go2Interface() @@ -294,26 +301,30 @@ def signal_handler(signal, frame): # Moving Camera Setting ############################################################ base_pos, base_ori = pb.getBasePositionAndOrientation(robot) - pb.resetDebugVisualizerCamera(cameraDistance=1.5, - cameraYaw=120, - cameraPitch=-30, - cameraTargetPosition=base_pos + - np.array([0.5, 0.3, -base_pos[2] + 0.5])) + pb.resetDebugVisualizerCamera( + cameraDistance=1.5, + cameraYaw=120, + cameraPitch=-30, + cameraTargetPosition=base_pos + np.array([0.5, 0.3, -base_pos[2] + 0.5]), + ) ############################################################################### # Debugging Purpose ############################################################################## ##debugging state estimator by calculating groundtruth basejoint states base_com_pos, base_com_quat = pb.getBasePositionAndOrientation(robot) rot_world_basecom = util.quat_to_rot(base_com_quat) - rot_world_basejoint = np.dot(rot_world_basecom, - rot_basejoint_to_basecom.transpose()) - base_joint_pos = base_com_pos - np.dot(rot_world_basejoint, - pos_basejoint_to_basecom) + rot_world_basejoint = np.dot( + rot_world_basecom, rot_basejoint_to_basecom.transpose() + ) + base_joint_pos = base_com_pos - np.dot( + rot_world_basejoint, pos_basejoint_to_basecom + ) base_joint_quat = util.rot_to_quat(rot_world_basejoint) base_com_lin_vel, base_com_ang_vel = pb.getBaseVelocity(robot) - trans_joint_com = liegroup.RpToTrans(rot_basejoint_to_basecom, - pos_basejoint_to_basecom) + trans_joint_com = liegroup.RpToTrans( + rot_basejoint_to_basecom, pos_basejoint_to_basecom + ) adjoint_joint_com = liegroup.Adjoint(trans_joint_com) twist_basecom_in_world = np.zeros(6) twist_basecom_in_world[0:3] = base_com_ang_vel @@ -321,15 +332,16 @@ def signal_handler(signal, frame): augrot_basecom_world = np.zeros((6, 6)) augrot_basecom_world[0:3, 0:3] = rot_world_basecom.transpose() augrot_basecom_world[3:6, 3:6] = rot_world_basecom.transpose() - twist_basecom_in_basecom = np.dot(augrot_basecom_world, - twist_basecom_in_world) - twist_basejoint_in_basejoint = np.dot(adjoint_joint_com, - twist_basecom_in_basecom) + twist_basecom_in_basecom = np.dot(augrot_basecom_world, twist_basecom_in_world) + twist_basejoint_in_basejoint = np.dot( + adjoint_joint_com, twist_basecom_in_basecom + ) augrot_world_basejoint = np.zeros((6, 6)) augrot_world_basejoint[0:3, 0:3] = rot_world_basejoint augrot_world_basejoint[3:6, 3:6] = rot_world_basejoint - twist_basejoint_in_world = np.dot(augrot_world_basejoint, - twist_basejoint_in_basejoint) + twist_basejoint_in_world = np.dot( + augrot_world_basejoint, twist_basejoint_in_basejoint + ) base_joint_ang_vel = twist_basejoint_in_world[0:3] base_joint_lin_vel = twist_basejoint_in_world[3:6] @@ -349,10 +361,21 @@ def signal_handler(signal, frame): ############################################################ # Get Sensor Data ############################################################ - (imu_frame_quat, imu_ang_vel, imu_dvel, joint_pos, joint_vel, - b_FL_foot_contact, b_FR_foot_contact, b_RL_foot_contact, - b_RR_foot_contact, FL_normal_force, FR_normal_force, RL_normal_force, - RR_normal_force) = get_sensor_data_from_pybullet(robot) + ( + imu_frame_quat, + imu_ang_vel, + imu_dvel, + joint_pos, + joint_vel, + b_FL_foot_contact, + b_FR_foot_contact, + b_RL_foot_contact, + b_RR_foot_contact, + FL_normal_force, + FR_normal_force, + RL_normal_force, + RR_normal_force, + ) = get_sensor_data_from_pybullet(robot) ## copy sensor data to rpc sensor data class rpc_go2_sensor_data.imu_frame_quat_ = imu_frame_quat @@ -390,8 +413,7 @@ def signal_handler(signal, frame): apply_control_input_to_pybullet(robot, rpc_trq_command) # save current torso velocity for next iteration - previous_torso_velocity = pybullet_util.get_link_vel( - robot, Go2LinkIdx.imu)[3:6] + previous_torso_velocity = pybullet_util.get_link_vel(robot, Go2LinkIdx.imu)[3:6] ############################################################ # Save Image file @@ -399,7 +421,8 @@ def signal_handler(signal, frame): if (Config.VIDEO_RECORD) and (count % Config.RECORD_FREQ == 0): camera_data = pb.getDebugVisualizerCamera() frame = pybullet_util.get_camera_image_from_debug_camera( - camera_data, Config.RENDER_WIDTH, Config.RENDER_HEIGHT) + camera_data, Config.RENDER_WIDTH, Config.RENDER_HEIGHT + ) filename = video_dir + "/step%06d.jpg" % jpg_count cv2.imwrite(filename, frame) jpg_count += 1 diff --git a/simulator/pybullet/optimo_main.py b/simulator/pybullet/optimo_main.py index ace9d083..058f8570 100644 --- a/simulator/pybullet/optimo_main.py +++ b/simulator/pybullet/optimo_main.py @@ -1,5 +1,4 @@ import pybullet as pb -import time import os from loop_rate_limiters import RateLimiter @@ -8,24 +7,18 @@ import sys sys.path.append(cwd) -sys.path.append(cwd + "/build/lib") #include pybind module +sys.path.append(cwd + "/build/lib") # include pybind module import numpy as np -import signal -import shutil -import cv2 from config.optimo.sim.pybullet.ihwbc.pybullet_params import * from util.python_utils import pybullet_util -from util.python_utils import util -from util.python_utils import liegroup import optimo_interface_py def get_sensor_data_from_pybullet(robot): - # Initialize sensor data joint_pos, joint_vel = np.zeros(7), np.zeros(7) @@ -54,20 +47,27 @@ def get_sensor_data_from_pybullet(robot): def set_init_config_pybullet(robot): - pb.resetJointState(robot, OptimoJointIdx.joint1, - Config.INITIAL_JOINT_POSITION[0], 0.0) - pb.resetJointState(robot, OptimoJointIdx.joint2, - Config.INITIAL_JOINT_POSITION[1], 0.0) - pb.resetJointState(robot, OptimoJointIdx.joint3, - Config.INITIAL_JOINT_POSITION[2], 0.0) - pb.resetJointState(robot, OptimoJointIdx.joint4, - Config.INITIAL_JOINT_POSITION[3], 0.0) - pb.resetJointState(robot, OptimoJointIdx.joint5, - Config.INITIAL_JOINT_POSITION[4], 0.0) - pb.resetJointState(robot, OptimoJointIdx.joint6, - Config.INITIAL_JOINT_POSITION[5], 0.0) - pb.resetJointState(robot, OptimoJointIdx.joint7, - Config.INITIAL_JOINT_POSITION[6], 0.0) + pb.resetJointState( + robot, OptimoJointIdx.joint1, Config.INITIAL_JOINT_POSITION[0], 0.0 + ) + pb.resetJointState( + robot, OptimoJointIdx.joint2, Config.INITIAL_JOINT_POSITION[1], 0.0 + ) + pb.resetJointState( + robot, OptimoJointIdx.joint3, Config.INITIAL_JOINT_POSITION[2], 0.0 + ) + pb.resetJointState( + robot, OptimoJointIdx.joint4, Config.INITIAL_JOINT_POSITION[3], 0.0 + ) + pb.resetJointState( + robot, OptimoJointIdx.joint5, Config.INITIAL_JOINT_POSITION[4], 0.0 + ) + pb.resetJointState( + robot, OptimoJointIdx.joint6, Config.INITIAL_JOINT_POSITION[5], 0.0 + ) + pb.resetJointState( + robot, OptimoJointIdx.joint7, Config.INITIAL_JOINT_POSITION[6], 0.0 + ) print("Initial Configuration Set") @@ -80,7 +80,7 @@ def every_time_step(count, interval): def add_tf_visualization(robot, link_index): - #Add a visualization of the desired frame of the robot. + # Add a visualization of the desired frame of the robot. pos, ori = pb.getLinkState(robot, link_index)[0:2] # Define the axis unit vectors @@ -110,34 +110,27 @@ def apply_control_input_to_pybullet(robot, command): mode = pb.TORQUE_CONTROL # Optimo Arm - pb.setJointMotorControl2(robot, - OptimoJointIdx.joint1, - controlMode=mode, - force=command[0]) - pb.setJointMotorControl2(robot, - OptimoJointIdx.joint2, - controlMode=mode, - force=command[1]) - pb.setJointMotorControl2(robot, - OptimoJointIdx.joint3, - controlMode=mode, - force=command[2]) - pb.setJointMotorControl2(robot, - OptimoJointIdx.joint4, - controlMode=mode, - force=command[3]) - pb.setJointMotorControl2(robot, - OptimoJointIdx.joint5, - controlMode=mode, - force=command[4]) - pb.setJointMotorControl2(robot, - OptimoJointIdx.joint6, - controlMode=mode, - force=command[5]) - pb.setJointMotorControl2(robot, - OptimoJointIdx.joint7, - controlMode=mode, - force=command[6]) + pb.setJointMotorControl2( + robot, OptimoJointIdx.joint1, controlMode=mode, force=command[0] + ) + pb.setJointMotorControl2( + robot, OptimoJointIdx.joint2, controlMode=mode, force=command[1] + ) + pb.setJointMotorControl2( + robot, OptimoJointIdx.joint3, controlMode=mode, force=command[2] + ) + pb.setJointMotorControl2( + robot, OptimoJointIdx.joint4, controlMode=mode, force=command[3] + ) + pb.setJointMotorControl2( + robot, OptimoJointIdx.joint5, controlMode=mode, force=command[4] + ) + pb.setJointMotorControl2( + robot, OptimoJointIdx.joint6, controlMode=mode, force=command[5] + ) + pb.setJointMotorControl2( + robot, OptimoJointIdx.joint7, controlMode=mode, force=command[6] + ) # Plato Hand # pb.setJointMotorControl2(robot, PlatoJointIdx.joint1, controlMode=mode, force=command[7]) @@ -154,16 +147,19 @@ def apply_control_input_to_pybullet(robot, command): def main(): # Pybullet Renderer pb.connect(pb.GUI) - pb.resetDebugVisualizerCamera(cameraDistance=1.5, - cameraYaw=0, - cameraPitch=-40, - cameraTargetPosition=[0, 0, 0.3]) + pb.resetDebugVisualizerCamera( + cameraDistance=1.5, + cameraYaw=0, + cameraPitch=-40, + cameraTargetPosition=[0, 0, 0.3], + ) pb.configureDebugVisualizer(pb.COV_ENABLE_GUI, 0) pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) # Simulation Physics - pb.setPhysicsEngineParameter(fixedTimeStep=Config.CONTROLLER_DT, - numSubSteps=Config.N_SUBSTEP) + pb.setPhysicsEngineParameter( + fixedTimeStep=Config.CONTROLLER_DT, numSubSteps=Config.N_SUBSTEP + ) pb.setGravity(0, 0, -9.81) ## robot spawn & initial kinematics and dynamics setting @@ -175,25 +171,37 @@ def main(): # Load URDF pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0) - robot = pb.loadURDF(cwd + "/robot_model/optimo/optimo.urdf", - Config.INITIAL_BASE_JOINT_POS, - Config.INITIAL_BASE_JOINT_QUAT, - useFixedBase=True) - ground = pb.loadURDF(cwd + "/robot_model/ground/plane.urdf", - useFixedBase=True) + robot = pb.loadURDF( + cwd + "/robot_model/optimo/optimo.urdf", + Config.INITIAL_BASE_JOINT_POS, + Config.INITIAL_BASE_JOINT_QUAT, + useFixedBase=True, + ) + ground = pb.loadURDF(cwd + "/robot_model/ground/plane.urdf", useFixedBase=True) # Initial joint configuration set_init_config_pybullet(robot) pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) - n_q, n_v, n_a, joint_id_dict, link_id_dict, pos_basejoint_to_basecom, rot_basejoint_to_basecom = pybullet_util.get_robot_config( - robot, Config.INITIAL_BASE_JOINT_POS, Config.INITIAL_BASE_JOINT_QUAT, - Config.PRINT_ROBOT_INFO) + ( + n_q, + n_v, + n_a, + joint_id_dict, + link_id_dict, + pos_basejoint_to_basecom, + rot_basejoint_to_basecom, + ) = pybullet_util.get_robot_config( + robot, + Config.INITIAL_BASE_JOINT_POS, + Config.INITIAL_BASE_JOINT_QUAT, + Config.PRINT_ROBOT_INFO, + ) # Joint Friction and Damping pybullet_util.set_joint_friction(robot, joint_id_dict, 0) - pybullet_util.set_link_damping(robot, link_id_dict, 0., 0.) + pybullet_util.set_link_damping(robot, link_id_dict, 0.0, 0.0) # Initialize RPC Interface rpc_optimo_interface = optimo_interface_py.OptimoInterface() @@ -205,29 +213,28 @@ def main(): rpc_optimo_sensor_data.base_quat_ = Config.INITIAL_BASE_JOINT_QUAT # Simulation Time Parameters - rate = RateLimiter(frequency=1. / dt) + rate = RateLimiter(frequency=1.0 / dt) # Simulation Main Loop while True: - ########################## Keyboard Interrupt ########################## keys = pb.getKeyboardEvents() - if pybullet_util.is_key_triggered(keys, '1'): + if pybullet_util.is_key_triggered(keys, "1"): rpc_optimo_interface.interrupt_.PressOne() print("Python: 1 Pressed") - elif pybullet_util.is_key_triggered(keys, '2'): + elif pybullet_util.is_key_triggered(keys, "2"): rpc_optimo_interface.interrupt_.PressTwo() - elif pybullet_util.is_key_triggered(keys, '4'): + elif pybullet_util.is_key_triggered(keys, "4"): rpc_optimo_interface.interrupt_.PressFour() - elif pybullet_util.is_key_triggered(keys, '5'): + elif pybullet_util.is_key_triggered(keys, "5"): rpc_optimo_interface.interrupt_.PressFive() - elif pybullet_util.is_key_triggered(keys, '6'): + elif pybullet_util.is_key_triggered(keys, "6"): rpc_optimo_interface.interrupt_.PressSix() - elif pybullet_util.is_key_triggered(keys, '7'): + elif pybullet_util.is_key_triggered(keys, "7"): rpc_optimo_interface.interrupt_.PressSeven() - elif pybullet_util.is_key_triggered(keys, '8'): + elif pybullet_util.is_key_triggered(keys, "8"): rpc_optimo_interface.interrupt_.PressEight() - elif pybullet_util.is_key_triggered(keys, '9'): + elif pybullet_util.is_key_triggered(keys, "9"): rpc_optimo_interface.interrupt_.PressNine() ########################## Sensor Update ############################## @@ -235,8 +242,7 @@ def main(): joint_pos, joint_vel = get_sensor_data_from_pybullet(robot) rpc_optimo_sensor_data.joint_pos_ = joint_pos rpc_optimo_sensor_data.joint_vel_ = joint_vel - rpc_optimo_interface.GetCommand(rpc_optimo_sensor_data, - rpc_optimo_command) + rpc_optimo_interface.GetCommand(rpc_optimo_sensor_data, rpc_optimo_command) ###################################################################### ########################## Command Update ############################## @@ -252,9 +258,11 @@ def main(): # print("Command vel: ", rpc_joint_vel_command, end="\r") # t_cmd = t_ff + kp(e) + kd(e_dot) - joint_impedance_command = rpc_trq_command + ActuatorGains.KP * ( - rpc_joint_pos_command - - joint_pos) + ActuatorGains.KD * (rpc_joint_vel_command - joint_vel) + joint_impedance_command = ( + rpc_trq_command + + ActuatorGains.KP * (rpc_joint_pos_command - joint_pos) + + ActuatorGains.KD * (rpc_joint_vel_command - joint_vel) + ) apply_control_input_to_pybullet(robot, joint_impedance_command) ###################################################################### diff --git a/util/python_utils/pybullet_util.py b/util/python_utils/pybullet_util.py index d8950795..05221c2a 100644 --- a/util/python_utils/pybullet_util.py +++ b/util/python_utils/pybullet_util.py @@ -14,10 +14,7 @@ np.set_printoptions(precision=5) -def get_robot_config(robot, - initial_pos=None, - initial_quat=None, - b_print_info=False): +def get_robot_config(robot, initial_pos=None, initial_quat=None, b_print_info=False): nq, nv, na, joint_id, link_id = 0, 0, 0, OrderedDict(), OrderedDict() link_id[(pb.getBodyInfo(robot)[0]).decode("utf-8")] = -1 for i in range(pb.getNumJoints(robot)): @@ -34,13 +31,12 @@ def get_robot_config(robot, base_pos, base_quat = pb.getBasePositionAndOrientation(robot) rot_world_com = util.quat_to_rot(base_quat) initial_pos = [0.0, 0.0, 0.0] if initial_pos is None else initial_pos - initial_quat = [0.0, 0.0, 0.0, 1.0 - ] if initial_quat is None else initial_quat + initial_quat = [0.0, 0.0, 0.0, 1.0] if initial_quat is None else initial_quat rot_world_basejoint = util.quat_to_rot(np.array(initial_quat)) - pos_basejoint_to_basecom = np.dot(rot_world_basejoint.transpose(), - base_pos - np.array(initial_pos)) - rot_basejoint_to_basecom = np.dot(rot_world_basejoint.transpose(), - rot_world_com) + pos_basejoint_to_basecom = np.dot( + rot_world_basejoint.transpose(), base_pos - np.array(initial_pos) + ) + rot_basejoint_to_basecom = np.dot(rot_world_basejoint.transpose(), rot_world_com) if b_print_info: print("=" * 80) @@ -76,8 +72,9 @@ def get_robot_config(robot, ) -def get_kinematics_config(robot, joint_id, link_id, open_chain_joints, - base_link, ee_link): +def get_kinematics_config( + robot, joint_id, link_id, open_chain_joints, base_link, ee_link +): joint_screws_in_ee = np.zeros((6, len(open_chain_joints))) ee_link_state = pb.getLinkState(robot, link_id[ee_link], 1, 1) if link_id[base_link] == -1: @@ -85,10 +82,12 @@ def get_kinematics_config(robot, joint_id, link_id, open_chain_joints, else: base_link_state = pb.getLinkState(robot, link_id[base_link], 1, 1) base_pos, base_quat = base_link_state[0], base_link_state[1] - T_w_b = liegroup.RpToTrans(util.quat_to_rot(np.array(base_quat)), - np.array(base_pos)) - T_w_ee = liegroup.RpToTrans(util.quat_to_rot(np.array(ee_link_state[1])), - np.array(ee_link_state[0])) + T_w_b = liegroup.RpToTrans( + util.quat_to_rot(np.array(base_quat)), np.array(base_pos) + ) + T_w_ee = liegroup.RpToTrans( + util.quat_to_rot(np.array(ee_link_state[1])), np.array(ee_link_state[0]) + ) T_b_ee = np.dot(liegroup.TransInv(T_w_b), T_w_ee) for i, joint_name in enumerate(open_chain_joints): joint_info = pb.getJointInfo(robot, joint_id[joint_name]) @@ -97,8 +96,9 @@ def get_kinematics_config(robot, joint_id, link_id, open_chain_joints, joint_axis = joint_info[13] screw_at_joint = np.zeros(6) link_state = pb.getLinkState(robot, link_id[link_name], 1, 1) - T_w_j = liegroup.RpToTrans(util.quat_to_rot(np.array(link_state[5])), - np.array(link_state[4])) + T_w_j = liegroup.RpToTrans( + util.quat_to_rot(np.array(link_state[5])), np.array(link_state[4]) + ) T_ee_j = np.dot(liegroup.TransInv(T_w_ee), T_w_j) Adj_ee_j = liegroup.Adjoint(T_ee_j) if joint_type == pb.JOINT_REVOLUTE: @@ -131,10 +131,9 @@ def get_link_vel(robot, link_idx): def set_link_damping(robot, link_id, lin_damping, ang_damping): for i in link_id.values(): - pb.changeDynamics(robot, - i, - linearDamping=lin_damping, - angularDamping=ang_damping) + pb.changeDynamics( + robot, i, linearDamping=lin_damping, angularDamping=ang_damping + ) def set_joint_friction(robot, joint_id, max_force=0): @@ -189,16 +188,17 @@ def draw_link_frame(robot, link_idx, linewidth=5.0, text=None): def set_motor_impedance(robot, joint_id, command, kp, kd): trq_applied = OrderedDict() for (joint_name, pos_des), (_, vel_des), (_, trq_des) in zip( - command["joint_positions_cmd_"].items(), - command["joint_velocities_cmd_"].items(), - command["joint_torques_cmd_"].items(), + command["joint_positions_cmd_"].items(), + command["joint_velocities_cmd_"].items(), + command["joint_torques_cmd_"].items(), ): joint_state = pb.getJointState(robot, joint_id[joint_name]) joint_pos, joint_vel = joint_state[0], joint_state[1] - trq_applied[joint_id[joint_name]] = (trq_des + kp[joint_name] * - (pos_des - joint_pos) + - kd[joint_name] * - (vel_des - joint_vel)) + trq_applied[joint_id[joint_name]] = ( + trq_des + + kp[joint_name] * (pos_des - joint_pos) + + kd[joint_name] * (vel_des - joint_vel) + ) pb.setJointMotorControlArray( robot, @@ -237,8 +237,7 @@ def set_motor_pos(robot, joint_id, pos_cmd): def set_motor_pos_vel(robot, joint_id, pos_cmd, vel_cmd): pos_applied = OrderedDict() vel_applied = OrderedDict() - for (joint_name, pos_des), (_, vel_des) in zip(pos_cmd.items(), - vel_cmd.items()): + for (joint_name, pos_des), (_, vel_des) in zip(pos_cmd.items(), vel_cmd.items()): pos_applied[joint_id[joint_name]] = pos_des vel_applied[joint_id[joint_name]] = vel_des @@ -251,8 +250,9 @@ def set_motor_pos_vel(robot, joint_id, pos_cmd, vel_cmd): ) -def get_sensor_data(robot, joint_id, link_id, pos_basejoint_to_basecom, - rot_basejoint_to_basecom): +def get_sensor_data( + robot, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom +): """ Parameters ---------- @@ -306,13 +306,14 @@ def get_sensor_data(robot, joint_id, link_id, pos_basejoint_to_basecom, sensor_data["base_com_ang_vel"] = np.asarray(base_com_ang_vel) rot_world_com = util.quat_to_rot(np.copy(sensor_data["base_com_quat"])) - rot_world_joint = np.dot(rot_world_com, - rot_basejoint_to_basecom.transpose()) + rot_world_joint = np.dot(rot_world_com, rot_basejoint_to_basecom.transpose()) sensor_data["base_joint_pos"] = sensor_data["base_com_pos"] - np.dot( - rot_world_joint, pos_basejoint_to_basecom) + rot_world_joint, pos_basejoint_to_basecom + ) sensor_data["base_joint_quat"] = util.rot_to_quat(rot_world_joint) - trans_joint_com = liegroup.RpToTrans(rot_basejoint_to_basecom, - pos_basejoint_to_basecom) + trans_joint_com = liegroup.RpToTrans( + rot_basejoint_to_basecom, pos_basejoint_to_basecom + ) adT_joint_com = liegroup.Adjoint(trans_joint_com) twist_com_in_world = np.zeros(6) twist_com_in_world[0:3] = np.copy(sensor_data["base_com_ang_vel"]) @@ -322,8 +323,7 @@ def get_sensor_data(robot, joint_id, link_id, pos_basejoint_to_basecom, augrot_com_world[3:6, 3:6] = rot_world_com.transpose() twist_com_in_com = np.dot(augrot_com_world, twist_com_in_world) twist_joint_in_joint = np.dot(adT_joint_com, twist_com_in_com) - rot_world_joint = np.dot(rot_world_com, - rot_basejoint_to_basecom.transpose()) + rot_world_joint = np.dot(rot_world_com, rot_basejoint_to_basecom.transpose()) augrot_world_joint = np.zeros((6, 6)) augrot_world_joint[0:3, 0:3] = rot_world_joint augrot_world_joint[3:6, 3:6] = rot_world_joint @@ -344,7 +344,7 @@ def get_sensor_data(robot, joint_id, link_id, pos_basejoint_to_basecom, def simulate_dVel_data(robot, link_id, previous_link_velocity): # calculate imu acceleration in world frame by numerical differentiation - torso_dvel = (get_link_vel(robot, link_id)[3:6] - previous_link_velocity) + torso_dvel = get_link_vel(robot, link_id)[3:6] - previous_link_velocity return torso_dvel @@ -363,11 +363,11 @@ def simulate_contact_sensor(force_sensor_measurement): return (force_sensor_measurement - voltage_bias) / voltage_to_force_map -def get_camera_image_from_link(robot, link, pic_width, pic_height, fov, - nearval, farval): +def get_camera_image_from_link( + robot, link, pic_width, pic_height, fov, nearval, farval +): aspect = pic_width / pic_height - projection_matrix = pb.computeProjectionMatrixFOV(fov, aspect, nearval, - farval) + projection_matrix = pb.computeProjectionMatrixFOV(fov, aspect, nearval, farval) link_info = pb.getLinkState(robot, link, 1, 1) # Get head link info link_pos = link_info[0] # Get link com pos wrt world link_ori = link_info[1] # Get link com ori wrt world @@ -380,8 +380,9 @@ def get_camera_image_from_link(robot, link, pic_width, pic_height, fov, camera_eye_pos = link_pos + np.dot(rot, 0.1 * global_camera_x_unit) camera_target_pos = link_pos + np.dot(rot, 1.0 * global_camera_x_unit) camera_up_vector = np.dot(rot, global_camera_z_unit) - view_matrix = pb.computeViewMatrix(camera_eye_pos, camera_target_pos, - camera_up_vector) # SE3_camera_to_world + view_matrix = pb.computeViewMatrix( + camera_eye_pos, camera_target_pos, camera_up_vector + ) # SE3_camera_to_world width, height, rgb_img, depth_img, seg_img = pb.getCameraImage( pic_width, # image width pic_height, # image height @@ -406,8 +407,7 @@ def add_sensor_noise(measurement, noise): def make_video(video_dir, delete_jpgs=True): images = [] - for file in tqdm(sorted(os.listdir(video_dir)), - desc="converting jpgs to gif"): + for file in tqdm(sorted(os.listdir(video_dir)), desc="converting jpgs to gif"): filename = video_dir + "/" + file im = cv2.imread(filename) im = im[:, :, [2, 1, 0]] # << BGR to RGB @@ -492,23 +492,20 @@ def set_config(robot, joint_id, base_pos, base_quat, joint_pos): pb.resetJointState(robot, joint_id[k], v, 0.0) -def get_point_cloud_data(depth_buffer, view_matrix, projection_matrix, d_hor, - d_ver): +def get_point_cloud_data(depth_buffer, view_matrix, projection_matrix, d_hor, d_ver): view_matrix = np.asarray(view_matrix).reshape([4, 4], order="F") - projection_matrix = np.asarray(projection_matrix).reshape([4, 4], - order="F") - trans_world_to_pix = np.linalg.inv( - np.matmul(projection_matrix, view_matrix)) + projection_matrix = np.asarray(projection_matrix).reshape([4, 4], order="F") + trans_world_to_pix = np.linalg.inv(np.matmul(projection_matrix, view_matrix)) # trans_camera_to_pix = np.linalg.inv(projection_matrix) img_height = (depth_buffer.shape)[0] img_width = (depth_buffer.shape)[1] wf_point_cloud_data = np.empty( - [np.int(img_height / d_ver), - np.int(img_width / d_hor), 3]) + [np.int(img_height / d_ver), np.int(img_width / d_hor), 3] + ) cf_point_cloud_data = np.empty( - [np.int(img_height / d_ver), - np.int(img_width / d_hor), 3]) + [np.int(img_height / d_ver), np.int(img_width / d_hor), 3] + ) for h in range(0, img_height, d_ver): for w in range(0, img_width, d_hor): @@ -518,14 +515,12 @@ def get_point_cloud_data(depth_buffer, view_matrix, projection_matrix, d_hor, pix_pos = np.asarray([x, y, z, 1]) point_in_world = np.matmul(trans_world_to_pix, pix_pos) # point_in_camera = np.matmul(trans_camera_to_pix, pix_pos) - wf_point_cloud_data[np.int(h / d_ver), - np.int(w / d_hor), :] = ( - point_in_world / - point_in_world[3])[:3] # world frame - - cf_point_cloud_data[np.int(h / d_ver), - np.int(w / d_hor), :] = ( - point_in_world / - point_in_world[3])[:3] # camera frame + wf_point_cloud_data[np.int(h / d_ver), np.int(w / d_hor), :] = ( + point_in_world / point_in_world[3] + )[:3] # world frame + + cf_point_cloud_data[np.int(h / d_ver), np.int(w / d_hor), :] = ( + point_in_world / point_in_world[3] + )[:3] # camera frame return wf_point_cloud_data, cf_point_cloud_data