Skip to content

[tiago]: add differential drive #174

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Mar 15, 2022
Merged

[tiago]: add differential drive #174

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 11 additions & 6 deletions src/examples/tiago.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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()) {
Expand All @@ -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();
Expand Down
15 changes: 14 additions & 1 deletion src/python/robot.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -534,7 +534,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>>&>(),
Expand Down
22 changes: 22 additions & 0 deletions src/robot_dart/robots/tiago.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
5 changes: 5 additions & 0 deletions src/robot_dart/robots/tiago.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
24 changes: 12 additions & 12 deletions utheque/tiago/tiago_steel.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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>
Expand Down Expand Up @@ -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"/>
Expand Down Expand Up @@ -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"/>
Expand Down Expand Up @@ -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"/>
Expand Down Expand Up @@ -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"/>
Expand Down