Merge pull request #174 from resibots/tiago_diff_drive

[tiago]: add differential drive

Author: costashatz

src/examples/tiago.cpp

@@ -7,9 +7,10 @@
 
 int main()
 {
-    double dt = 0.01;
+    double dt = 0.001;
     robot_dart::RobotDARTSimu simu(dt);
-    simu.set_collision_detector("fcl");
+    // this is important for wheel collision; with FCL it does not work well
+    simu.set_collision_detector("bullet");
 
     size_t freq = 1. / dt;
     auto robot = std::make_shared<robot_dart::robots::Tiago>(freq);
@@ -29,14 +30,15 @@ int main()
     simu.add_robot(robot);
 
     simu.set_control_freq(100);
-    std::vector<std::string> dofs = {"arm_1_joint",
+    std::vector<std::string> wheel_dofs = {"wheel_right_joint", "wheel_left_joint"};
+    std::vector<std::string> arm_dofs = {"arm_1_joint",
         "arm_2_joint",
         "arm_3_joint",
         "arm_4_joint",
         "arm_5_joint",
         "torso_lift_joint"};
 
-    Eigen::VectorXd init_positions = robot->positions(dofs);
+    Eigen::VectorXd init_positions = robot->positions(arm_dofs);
 
     auto start = std::chrono::steady_clock::now();
     while (simu.scheduler().next_time() < 20. && !simu.graphics()->done()) {
@@ -48,8 +50,11 @@ int main()
                 sin(simu.scheduler().current_time() * 2.),
                 sin(simu.scheduler().current_time() * 2.),
                 sin(simu.scheduler().current_time() * 2.);
-            Eigen::VectorXd commands = (init_positions + delta_pos) - robot->positions(dofs);
-            robot->set_commands(commands, dofs);
+            Eigen::VectorXd commands = (init_positions + delta_pos) - robot->positions(arm_dofs);
+            robot->set_commands(commands, arm_dofs);
+            Eigen::VectorXd cmd(wheel_dofs.size());
+            cmd << 0., 5.;
+            robot->set_commands(cmd, wheel_dofs);
         }
 
         simu.step_world();

src/python/robot.cpp

@@ -535,7 +535,20 @@ namespace robot_dart {
                     py::arg("frequency") = 1000,
                     py::arg("urdf") = "tiago/tiago_steel.urdf",
                     py::arg("packages") = std::vector<std::pair<std::string, std::string>>({{"tiago_description", "tiago/tiago_description"}}))
-                .def("ft_wrist", &Tiago::ft_wrist, py::return_value_policy::reference);
+                .def("ft_wrist", &Tiago::ft_wrist, py::return_value_policy::reference)
+                .def("caster_joints", &Tiago::caster_joints)
+                .def("set_actuator_types", &Tiago::set_actuator_types,
+                    py::arg("type"),
+                    py::arg("joint_names") = std::vector<std::string>(),
+                    py::arg("override_mimic") = false,
+                    py::arg("override_base") = false,
+                    py::arg("override_caster") = false)
+                .def("set_actuator_type", &Tiago::set_actuator_type,
+                    py::arg("type"),
+                    py::arg("joint_name"),
+                    py::arg("override_mimic") = false,
+                    py::arg("override_base") = false,
+                    py::arg("override_caster") = false);
 
             py::class_<ICub, Robot, std::shared_ptr<ICub>>(m, "ICub")
                 .def(py::init<size_t, const std::string&, const std::vector<std::pair<std::string, std::string>>&>(),

src/robot_dart/robots/tiago.cpp

@@ -10,9 +10,31 @@ namespace robot_dart {
             skeleton()->setPosition(2, M_PI / 2.);
             skeleton()->setPosition(5, 0.01);
             set_position_enforced(true);
+            // We use servo actuators, but not for the caster joints
             set_actuator_types("servo");
         }
 
+        void Tiago::set_actuator_types(const std::string& type, const std::vector<std::string>& joint_names, bool override_mimic, bool override_base, bool override_caster)
+        {
+            auto jt = joint_names;
+            if (!override_caster) {
+                if (joint_names.size() == 0)
+                    jt = Robot::joint_names();
+                for (const auto& jnt : caster_joints()) {
+                    auto it = std::find(jt.begin(), jt.end(), jnt);
+                    if (it != jt.end()) {
+                        jt.erase(it);
+                    }
+                }
+            }
+            Robot::set_actuator_types(type, jt, override_mimic, override_base);
+        }
+
+        void Tiago::set_actuator_type(const std::string& type, const std::string& joint_name, bool override_mimic, bool override_base, bool override_caster)
+        {
+            set_actuator_types(type, {joint_name}, override_mimic, override_base, override_caster);
+        }
+
         void Tiago::_post_addition(RobotDARTSimu* simu)
         {
             // We do not want to add sensors if we are a ghost robot

src/robot_dart/robots/tiago.hpp

@@ -13,6 +13,11 @@ namespace robot_dart {
 
             const sensor::ForceTorque& ft_wrist() const { return *_ft_wrist; }
 
+            std::vector<std::string> caster_joints() const { return {"caster_back_left_2_joint", "caster_back_left_1_joint", "caster_back_right_2_joint", "caster_back_right_1_joint", "caster_front_left_2_joint", "caster_front_left_1_joint", "caster_front_right_2_joint", "caster_front_right_1_joint"}; }
+
+            void set_actuator_types(const std::string& type, const std::vector<std::string>& joint_names = {}, bool override_mimic = false, bool override_base = false, bool override_caster = false);
+            void set_actuator_type(const std::string& type, const std::string& joint_name, bool override_mimic = false, bool override_base = false, bool override_caster = false);
+
         protected:
             std::shared_ptr<sensor::ForceTorque> _ft_wrist;
             void _post_addition(RobotDARTSimu* simu) override;

utheque/tiago/tiago_steel.urdf

@@ -311,12 +311,12 @@
     <dynamics damping="100"/>
     <safety_controller k_position="0" k_velocity="10" soft_lower_limit="-0.005" soft_upper_limit="0.005"/>
   </joint>
-  <joint name="wheel_right_joint" type="revolute">
+  <joint name="wheel_right_joint" type="continuous">
     <parent link="suspension_right_link"/>
     <child link="wheel_right_link"/>
     <origin rpy="-1.57079632679 0 0" xyz="0 -0.2022 0"/>
     <axis xyz="0 0 1"/>
-    <limit effort="6.0" velocity="10.152284264" lower="-1.7976931348623157e+308" upper="1.7976931348623157e+308"/>
+    <limit effort="6.0" velocity="10.152284264"/>
   </joint>
   <transmission name="wheel_right_trans">
     <type>transmission_interface/SimpleTransmission</type>
@@ -368,12 +368,12 @@
     <dynamics damping="100"/>
     <safety_controller k_position="0" k_velocity="10" soft_lower_limit="-0.005" soft_upper_limit="0.005"/>
   </joint>
-  <joint name="wheel_left_joint" type="revolute">
+  <joint name="wheel_left_joint" type="continuous">
     <parent link="suspension_left_link"/>
     <child link="wheel_left_link"/>
     <origin rpy="-1.57079632679 0 0" xyz="0 0.2022 0"/>
     <axis xyz="0 0 1"/>
-    <limit effort="6.0" velocity="10.152284264" lower="-1.7976931348623157e+308" upper="1.7976931348623157e+308"/>
+    <limit effort="6.0" velocity="10.152284264"/>
   </joint>
   <transmission name="wheel_left_trans">
     <type>transmission_interface/SimpleTransmission</type>
@@ -425,14 +425,14 @@
       </geometry>
     </collision>
   </link>
-  <joint name="caster_front_right_1_joint" type="fixed">
+  <joint name="caster_front_right_1_joint" type="continuous">
     <parent link="base_link"/>
     <child link="caster_front_right_1_link"/>
     <origin rpy="0 0 0" xyz="0.1695 -0.102 -0.0335"/>
     <axis xyz="0 0 1"/>
     <dynamics damping="0.005" friction="0.0"/>
   </joint>
-  <joint name="caster_front_right_2_joint" type="fixed">
+  <joint name="caster_front_right_2_joint" type="continuous">
     <parent link="caster_front_right_1_link"/>
     <child link="caster_front_right_2_link"/>
     <origin rpy="-1.57079632679 0 0" xyz="-0.016 0.0000 -0.040"/>
@@ -478,14 +478,14 @@
       </geometry>
     </collision>
   </link>
-  <joint name="caster_front_left_1_joint" type="fixed">
+  <joint name="caster_front_left_1_joint" type="continuous">
     <parent link="base_link"/>
     <child link="caster_front_left_1_link"/>
     <origin rpy="0 0 0" xyz="0.1695 0.102 -0.0335"/>
     <axis xyz="0 0 1"/>
     <dynamics damping="0.005" friction="0.0"/>
   </joint>
-  <joint name="caster_front_left_2_joint" type="fixed">
+  <joint name="caster_front_left_2_joint" type="continuous">
     <parent link="caster_front_left_1_link"/>
     <child link="caster_front_left_2_link"/>
     <origin rpy="-1.57079632679 0 0" xyz="-0.016 0.0000 -0.040"/>
@@ -531,14 +531,14 @@
       </geometry>
     </collision>
   </link>
-  <joint name="caster_back_right_1_joint" type="fixed">
+  <joint name="caster_back_right_1_joint" type="continuous">
     <parent link="base_link"/>
     <child link="caster_back_right_1_link"/>
     <origin rpy="0 0 0" xyz="-0.1735 -0.102 -0.0335"/>
     <axis xyz="0 0 1"/>
     <dynamics damping="0.005" friction="0.0"/>
   </joint>
-  <joint name="caster_back_right_2_joint" type="fixed">
+  <joint name="caster_back_right_2_joint" type="continuous">
     <parent link="caster_back_right_1_link"/>
     <child link="caster_back_right_2_link"/>
     <origin rpy="-1.57079632679 0 0" xyz="-0.016 0.0000 -0.040"/>
@@ -584,14 +584,14 @@
       </geometry>
     </collision>
   </link>
-  <joint name="caster_back_left_1_joint" type="fixed">
+  <joint name="caster_back_left_1_joint" type="continuous">
     <parent link="base_link"/>
     <child link="caster_back_left_1_link"/>
     <origin rpy="0 0 0" xyz="-0.1735 0.102 -0.0335"/>
     <axis xyz="0 0 1"/>
     <dynamics damping="0.005" friction="0.0"/>
   </joint>
-  <joint name="caster_back_left_2_joint" type="fixed">
+  <joint name="caster_back_left_2_joint" type="continuous">
     <parent link="caster_back_left_1_link"/>
     <child link="caster_back_left_2_link"/>
     <origin rpy="-1.57079632679 0 0" xyz="-0.016 0.0000 -0.040"/>