robot-control-stack/src/sim/SimRobot.h at 4a3f8ea08ed328ef78b0eae6d953e9d01081237f · RobotControlStack/robot-control-stack · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
#ifndef RCS_SIMROBOT_H
#define RCS_SIMROBOT_H
#include <mujoco/mujoco.h>
#include <rcs/Kinematics.h>
#include <rcs/Pose.h>
#include <rcs/Robot.h>
#include <rcs/utils.h>

#include "sim/sim.h"

namespace rcs {
namespace sim {

struct SimRobotConfig : common::RobotConfig {
  double joint_rotational_tolerance =
      .05 * (std::numbers::pi / 180.0);    // 0.05 degree
  double seconds_between_callbacks = 0.1;  // 10 Hz
  bool trajectory_trace = false;
  std::vector<std::string> arm_collision_geoms{
      "fr3_link0_collision", "fr3_link1_collision", "fr3_link2_collision",
      "fr3_link3_collision", "fr3_link4_collision", "fr3_link5_collision",
      "fr3_link6_collision", "fr3_link7_collision"};
  std::vector<std::string> joints = {
      "fr3_joint1", "fr3_joint2", "fr3_joint3", "fr3_joint4",
      "fr3_joint5", "fr3_joint6", "fr3_joint7",
  };
  std::vector<std::string> actuators = {
      "fr3_joint1", "fr3_joint2", "fr3_joint3", "fr3_joint4",
      "fr3_joint5", "fr3_joint6", "fr3_joint7",
  };
  std::string base = "base";
  std::string mjcf_scene_path = "assets/scenes/fr3_empty_world/scene.xml";

  void add_id(const std::string &id) {
    for (auto &s : this->arm_collision_geoms) {
      s = s + "_" + id;
    }
    for (auto &s : this->joints) {
      s = s + "_" + id;
    }
    for (auto &s : this->actuators) {
      s = s + "_" + id;
    }
    this->attachment_site = this->attachment_site + "_" + id;
    this->base = this->base + "_" + id;
  }
};

struct SimRobotState : common::RobotState {
  common::VectorXd previous_angles;
  common::VectorXd target_angles;
  common::Pose inverse_tcp_offset;
  bool ik_success = true;
  bool collision = false;
  bool is_moving = false;
  bool is_arrived = false;
};

class SimRobot : public common::Robot {
 public:
  SimRobot(std::shared_ptr<rcs::sim::Sim> sim,
           std::shared_ptr<common::Kinematics> ik, SimRobotConfig cfg,
           bool register_convergence_callback = true);
  ~SimRobot() override;
  bool set_config(const SimRobotConfig &cfg);
  SimRobotConfig *get_config() override;
  SimRobotState *get_state() override;
  common::Pose get_cartesian_position() override;
  void set_joint_position(const common::VectorXd &q) override;
  common::VectorXd get_joint_position() override;
  void move_home() override;
  void set_cartesian_position(const common::Pose &pose) override;
  common::Pose get_base_pose_in_world_coordinates() override;
  std::optional<std::shared_ptr<common::Kinematics>> get_ik() override;
  void reset() override;
  void set_joints_hard(const common::VectorXd &q);
  void clear_collision_flag();
  void close() override {};

 private:
  SimRobotConfig cfg;
  SimRobotState state;
  std::shared_ptr<Sim> sim;
  std::shared_ptr<common::Kinematics> m_ik;
  struct {
    std::set<size_t> cgeom;
    int attachment_site;
    std::vector<int> joints;
    std::vector<int> actuators;
    int base;
  } ids;
  void is_moving_callback();
  void is_arrived_callback();
  bool collision_callback();
  bool convergence_callback();
  void init_ids();
  void construct();
  void m_reset();
  common::VectorXd m_get_joint_position();
};
}  // namespace sim
}  // namespace rcs
#endif  // RCS_SIMROBOT_H