Skip to content

Commit f359dde

Browse files
costashatzcostashatz
authored
Merge pull request #147 from resibots/remove_damages
Remove Damages
2 parents 299dbc8 + 94b5134 commit f359dde

File tree

5 files changed

+31
-64
lines changed

5 files changed

+31
-64
lines changed

‎src/examples/a1.cpp

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -15,9 +15,14 @@ int main()
1515
robot->set_color_mode("material");
1616

1717
robot->set_self_collision(true);
18-
// robot->set_actuator_types("servo");
18+
robot->set_actuator_types("servo");
1919
robot->set_position_enforced(true);
20-
robot->set_base_pose(robot_dart::make_vector({0.55, 0.3, 0., 0., 0., 1.}));
20+
robot->set_base_pose(robot_dart::make_vector({0., 0., 0., 0., 0., 0.5}));
21+
22+
auto names = robot->dof_names(true, true, true);
23+
names = std::vector<std::string>(names.begin() + 6, names.end());
24+
// standing pose
25+
robot->set_positions(robot_dart::make_vector({0.0, 0.67, -1.3, -0.0, 0.67, -1.3, 0.0, 0.67, -1.3, -0.0, 0.67, -1.3}), names);
2126

2227
robot_dart::RobotDARTSimu simu(0.001);
2328
simu.set_collision_detector("fcl");

‎src/examples/pendulum.cpp

Lines changed: 0 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -9,21 +9,6 @@
99

1010
int main()
1111
{
12-
// std::vector<robot_dart::RobotDamage> brk = {};
13-
// Examples of damages
14-
// robot_dart::RobotDamage dmg;
15-
// dmg.type = "blocked_joint";
16-
// dmg.data = "arm_joint_4";
17-
// dmg.extra = new double(1.0);
18-
// brk.push_back(dmg);
19-
// dmg.type = "blocked_joint";
20-
// dmg.data = "arm_joint_2";
21-
// dmg.extra = nullptr;
22-
// brk.push_back(dmg);
23-
// dmg.type = "blocked_joint";
24-
// dmg.data = "arm_joint_3";
25-
// brk.push_back(dmg);
26-
2712
auto robot = std::make_shared<robot_dart::Robot>("pendulum.urdf");
2813
robot->fix_to_world();
2914
robot->set_position_enforced(false);

‎src/robot_dart/robot.cpp

Lines changed: 6 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -263,24 +263,24 @@ namespace robot_dart {
263263
}
264264
} // namespace detail
265265

266-
Robot::Robot(const std::string& model_file, const std::vector<std::pair<std::string, std::string>>& packages, const std::string& robot_name, bool is_urdf_string, bool cast_shadows, std::vector<RobotDamage> damages)
266+
Robot::Robot(const std::string& model_file, const std::vector<std::pair<std::string, std::string>>& packages, const std::string& robot_name, bool is_urdf_string, bool cast_shadows)
267267
: _robot_name(robot_name), _skeleton(_load_model(model_file, packages, is_urdf_string)), _cast_shadows(cast_shadows), _is_ghost(false)
268268
{
269269
ROBOT_DART_EXCEPTION_INTERNAL_ASSERT(_skeleton != nullptr);
270-
_set_damages(damages);
270+
update_joint_dof_maps();
271271
}
272272

273-
Robot::Robot(const std::string& model_file, const std::string& robot_name, bool is_urdf_string, bool cast_shadows, std::vector<RobotDamage> damages)
274-
: Robot(model_file, std::vector<std::pair<std::string, std::string>>(), robot_name, is_urdf_string, cast_shadows, damages)
273+
Robot::Robot(const std::string& model_file, const std::string& robot_name, bool is_urdf_string, bool cast_shadows)
274+
: Robot(model_file, std::vector<std::pair<std::string, std::string>>(), robot_name, is_urdf_string, cast_shadows)
275275
{
276276
}
277277

278-
Robot::Robot(dart::dynamics::SkeletonPtr skeleton, const std::string& robot_name, bool cast_shadows, std::vector<RobotDamage> damages)
278+
Robot::Robot(dart::dynamics::SkeletonPtr skeleton, const std::string& robot_name, bool cast_shadows)
279279
: _robot_name(robot_name), _skeleton(skeleton), _cast_shadows(cast_shadows), _is_ghost(false)
280280
{
281281
ROBOT_DART_EXCEPTION_INTERNAL_ASSERT(_skeleton != nullptr);
282282
_skeleton->setName(robot_name);
283-
_set_damages(damages);
283+
update_joint_dof_maps();
284284
reset();
285285
}
286286

@@ -295,7 +295,6 @@ namespace robot_dart {
295295
#endif
296296
_skeleton->getMutex().unlock();
297297
auto robot = std::make_shared<Robot>(tmp_skel, _robot_name);
298-
robot->_damages = _damages;
299298
robot->_model_filename = _model_filename;
300299
robot->_controllers.clear();
301300
for (auto& ctrl : _controllers) {
@@ -315,7 +314,6 @@ namespace robot_dart {
315314
#endif
316315
_skeleton->getMutex().unlock();
317316
auto robot = std::make_shared<Robot>(tmp_skel, ghost_name + "_" + _robot_name);
318-
robot->_damages = _damages;
319317
robot->_model_filename = _model_filename;
320318

321319
// ghost robots have no controllers
@@ -373,8 +371,6 @@ namespace robot_dart {
373371
return _skeleton->getDof(dof_index);
374372
}
375373

376-
std::vector<RobotDamage> Robot::damages() const { return _damages; }
377-
378374
const std::string& Robot::name() const { return _robot_name; }
379375

380376
void Robot::update(double t)
@@ -1941,24 +1937,6 @@ namespace robot_dart {
19411937
return tmp_skel;
19421938
}
19431939

1944-
void Robot::_set_damages(const std::vector<RobotDamage>& damages)
1945-
{
1946-
_damages = damages;
1947-
for (auto dmg : _damages) {
1948-
if (dmg.type == "blocked_joint") {
1949-
auto jnt = _skeleton->getJoint(dmg.data);
1950-
if (dmg.extra)
1951-
jnt->setPosition(0, *((double*)dmg.extra));
1952-
jnt->setActuatorType(dart::dynamics::Joint::LOCKED);
1953-
}
1954-
else if (dmg.type == "free_joint") {
1955-
_skeleton->getJoint(dmg.data)->setActuatorType(dart::dynamics::Joint::PASSIVE);
1956-
}
1957-
}
1958-
1959-
update_joint_dof_maps();
1960-
}
1961-
19621940
void Robot::_set_color_mode(dart::dynamics::MeshShape::ColorMode color_mode, dart::dynamics::SkeletonPtr skel)
19631941
{
19641942
for (size_t i = 0; i < skel->getNumBodyNodes(); ++i) {

‎src/robot_dart/robot.hpp

Lines changed: 3 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -10,23 +10,12 @@ namespace robot_dart {
1010
namespace control {
1111
class RobotControl;
1212
}
13-
struct RobotDamage {
14-
RobotDamage() {}
15-
RobotDamage(const std::string& type, const std::string& data, void* extra = nullptr)
16-
: type(type), data(data), extra(extra)
17-
{
18-
}
19-
20-
std::string type;
21-
std::string data;
22-
void* extra = nullptr;
23-
};
2413

2514
class Robot : public std::enable_shared_from_this<Robot> {
2615
public:
27-
Robot(const std::string& model_file, const std::vector<std::pair<std::string, std::string>>& packages, const std::string& robot_name = "robot", bool is_urdf_string = false, bool cast_shadows = true, std::vector<RobotDamage> damages = {});
28-
Robot(const std::string& model_file, const std::string& robot_name = "robot", bool is_urdf_string = false, bool cast_shadows = true, std::vector<RobotDamage> damages = {});
29-
Robot(dart::dynamics::SkeletonPtr skeleton, const std::string& robot_name = "robot", bool cast_shadows = true, std::vector<RobotDamage> damages = {});
16+
Robot(const std::string& model_file, const std::vector<std::pair<std::string, std::string>>& packages, const std::string& robot_name = "robot", bool is_urdf_string = false, bool cast_shadows = true);
17+
Robot(const std::string& model_file, const std::string& robot_name = "robot", bool is_urdf_string = false, bool cast_shadows = true);
18+
Robot(dart::dynamics::SkeletonPtr skeleton, const std::string& robot_name = "robot", bool cast_shadows = true);
3019

3120
std::shared_ptr<Robot> clone() const;
3221
std::shared_ptr<Robot> clone_ghost(const std::string& ghost_name = "ghost", const Eigen::Vector4d& ghost_color = {0.3, 0.3, 0.3, 0.7}) const;
@@ -41,8 +30,6 @@ namespace robot_dart {
4130
dart::dynamics::DegreeOfFreedom* dof(const std::string& dof_name);
4231
dart::dynamics::DegreeOfFreedom* dof(size_t dof_index);
4332

44-
std::vector<RobotDamage> damages() const;
45-
4633
const std::string& name() const;
4734
// to use the same urdf somewhere else
4835
const std::string& model_filename() const { return _model_filename; }
@@ -300,7 +287,6 @@ namespace robot_dart {
300287
std::string _get_path(const std::string& filename) const;
301288
dart::dynamics::SkeletonPtr _load_model(const std::string& filename, const std::vector<std::pair<std::string, std::string>>& packages = std::vector<std::pair<std::string, std::string>>(), bool is_urdf_string = false);
302289

303-
void _set_damages(const std::vector<RobotDamage>& damages);
304290
void _set_color_mode(dart::dynamics::MeshShape::ColorMode color_mode, dart::dynamics::SkeletonPtr skel);
305291
void _set_color_mode(dart::dynamics::MeshShape::ColorMode color_mode, dart::dynamics::ShapeNode* sn);
306292
void _set_actuator_type(size_t joint_index, dart::dynamics::Joint::ActuatorType type, bool override_mimic = false, bool override_base = false);
@@ -317,7 +303,6 @@ namespace robot_dart {
317303
std::string _model_filename;
318304
std::vector<std::pair<std::string, std::string>> _packages;
319305
dart::dynamics::SkeletonPtr _skeleton;
320-
std::vector<RobotDamage> _damages;
321306
std::vector<std::shared_ptr<control::RobotControl>> _controllers;
322307
std::unordered_map<std::string, size_t> _dof_map, _joint_map;
323308
bool _cast_shadows;

‎src/tests/test_robot.cpp

Lines changed: 15 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -45,8 +45,22 @@ BOOST_AUTO_TEST_CASE(test_constructors)
4545
BOOST_CHECK(dummy->name() == "dummy_robot");
4646
BOOST_CHECK(dummy->name() == dummy->skeleton()->getName());
4747
BOOST_CHECK(dummy->name() == dummy_skel->getName());
48+
}
49+
50+
BOOST_AUTO_TEST_CASE(test_dof_maps)
51+
{
52+
auto pendulum = std::make_shared<Robot>(std::string(ROBOT_DART_BUILD_DIR) + "/robots/pendulum.urdf");
53+
BOOST_REQUIRE(pendulum);
54+
55+
// check dofs
56+
auto names = pendulum->dof_names();
57+
BOOST_CHECK(names.size() == 7);
58+
59+
// check if joint/dof map is updated
60+
std::vector<std::string> name = {"pendulum_joint_1"};
61+
pendulum->set_positions(make_vector({0.1}), name);
4862

49-
// TO-DO: Add checks for damages
63+
BOOST_CHECK(pendulum->positions(name)[0] == 0.1);
5064
}
5165

5266
BOOST_AUTO_TEST_CASE(test_fix_free)

0 commit comments

Comments
 (0)