Skip to content
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

Remove Damages #147

Merged
merged 3 commits into from
Sep 28, 2021
Merged

Remove Damages #147

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
03584bc
[damages]: remove completely
costashatz Sep 28, 2021
7126d4b
[damage]: fix joint_dof_map
costashatz Sep 28, 2021
3fc0409
Fix tests
costashatz Sep 28, 2021
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
9 changes: 7 additions & 2 deletions src/examples/a1.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,9 +15,14 @@ int main()
robot->set_color_mode("material");

robot->set_self_collision(true);
// robot->set_actuator_types("servo");
robot->set_actuator_types("servo");
robot->set_position_enforced(true);
robot->set_base_pose(robot_dart::make_vector({0.55, 0.3, 0., 0., 0., 1.}));
robot->set_base_pose(robot_dart::make_vector({0., 0., 0., 0., 0., 0.5}));

auto names = robot->dof_names(true, true, true);
names = std::vector<std::string>(names.begin() + 6, names.end());
// standing pose
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);

robot_dart::RobotDARTSimu simu(0.001);
simu.set_collision_detector("fcl");
Expand Down
15 changes: 0 additions & 15 deletions src/examples/pendulum.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,21 +9,6 @@

int main()
{
// std::vector<robot_dart::RobotDamage> brk = {};
// Examples of damages
// robot_dart::RobotDamage dmg;
// dmg.type = "blocked_joint";
// dmg.data = "arm_joint_4";
// dmg.extra = new double(1.0);
// brk.push_back(dmg);
// dmg.type = "blocked_joint";
// dmg.data = "arm_joint_2";
// dmg.extra = nullptr;
// brk.push_back(dmg);
// dmg.type = "blocked_joint";
// dmg.data = "arm_joint_3";
// brk.push_back(dmg);

auto robot = std::make_shared<robot_dart::Robot>("pendulum.urdf");
robot->fix_to_world();
robot->set_position_enforced(false);
Expand Down
34 changes: 6 additions & 28 deletions src/robot_dart/robot.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -263,24 +263,24 @@ namespace robot_dart {
}
} // namespace detail

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)
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)
: _robot_name(robot_name), _skeleton(_load_model(model_file, packages, is_urdf_string)), _cast_shadows(cast_shadows), _is_ghost(false)
{
ROBOT_DART_EXCEPTION_INTERNAL_ASSERT(_skeleton != nullptr);
_set_damages(damages);
update_joint_dof_maps();
}

Robot::Robot(const std::string& model_file, const std::string& robot_name, bool is_urdf_string, bool cast_shadows, std::vector<RobotDamage> damages)
: Robot(model_file, std::vector<std::pair<std::string, std::string>>(), robot_name, is_urdf_string, cast_shadows, damages)
Robot::Robot(const std::string& model_file, const std::string& robot_name, bool is_urdf_string, bool cast_shadows)
: Robot(model_file, std::vector<std::pair<std::string, std::string>>(), robot_name, is_urdf_string, cast_shadows)
{
}

Robot::Robot(dart::dynamics::SkeletonPtr skeleton, const std::string& robot_name, bool cast_shadows, std::vector<RobotDamage> damages)
Robot::Robot(dart::dynamics::SkeletonPtr skeleton, const std::string& robot_name, bool cast_shadows)
: _robot_name(robot_name), _skeleton(skeleton), _cast_shadows(cast_shadows), _is_ghost(false)
{
ROBOT_DART_EXCEPTION_INTERNAL_ASSERT(_skeleton != nullptr);
_skeleton->setName(robot_name);
_set_damages(damages);
update_joint_dof_maps();
reset();
}

Expand All @@ -295,7 +295,6 @@ namespace robot_dart {
#endif
_skeleton->getMutex().unlock();
auto robot = std::make_shared<Robot>(tmp_skel, _robot_name);
robot->_damages = _damages;
robot->_model_filename = _model_filename;
robot->_controllers.clear();
for (auto& ctrl : _controllers) {
Expand All @@ -315,7 +314,6 @@ namespace robot_dart {
#endif
_skeleton->getMutex().unlock();
auto robot = std::make_shared<Robot>(tmp_skel, ghost_name + "_" + _robot_name);
robot->_damages = _damages;
robot->_model_filename = _model_filename;

// ghost robots have no controllers
Expand Down Expand Up @@ -373,8 +371,6 @@ namespace robot_dart {
return _skeleton->getDof(dof_index);
}

std::vector<RobotDamage> Robot::damages() const { return _damages; }

const std::string& Robot::name() const { return _robot_name; }

void Robot::update(double t)
Expand Down Expand Up @@ -1941,24 +1937,6 @@ namespace robot_dart {
return tmp_skel;
}

void Robot::_set_damages(const std::vector<RobotDamage>& damages)
{
_damages = damages;
for (auto dmg : _damages) {
if (dmg.type == "blocked_joint") {
auto jnt = _skeleton->getJoint(dmg.data);
if (dmg.extra)
jnt->setPosition(0, *((double*)dmg.extra));
jnt->setActuatorType(dart::dynamics::Joint::LOCKED);
}
else if (dmg.type == "free_joint") {
_skeleton->getJoint(dmg.data)->setActuatorType(dart::dynamics::Joint::PASSIVE);
}
}

update_joint_dof_maps();
}

void Robot::_set_color_mode(dart::dynamics::MeshShape::ColorMode color_mode, dart::dynamics::SkeletonPtr skel)
{
for (size_t i = 0; i < skel->getNumBodyNodes(); ++i) {
Expand Down
21 changes: 3 additions & 18 deletions src/robot_dart/robot.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,23 +10,12 @@ namespace robot_dart {
namespace control {
class RobotControl;
}
struct RobotDamage {
RobotDamage() {}
RobotDamage(const std::string& type, const std::string& data, void* extra = nullptr)
: type(type), data(data), extra(extra)
{
}

std::string type;
std::string data;
void* extra = nullptr;
};

class Robot : public std::enable_shared_from_this<Robot> {
public:
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 = {});
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 = {});
Robot(dart::dynamics::SkeletonPtr skeleton, const std::string& robot_name = "robot", bool cast_shadows = true, std::vector<RobotDamage> damages = {});
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);
Robot(const std::string& model_file, const std::string& robot_name = "robot", bool is_urdf_string = false, bool cast_shadows = true);
Robot(dart::dynamics::SkeletonPtr skeleton, const std::string& robot_name = "robot", bool cast_shadows = true);

std::shared_ptr<Robot> clone() const;
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;
Expand All @@ -41,8 +30,6 @@ namespace robot_dart {
dart::dynamics::DegreeOfFreedom* dof(const std::string& dof_name);
dart::dynamics::DegreeOfFreedom* dof(size_t dof_index);

std::vector<RobotDamage> damages() const;

const std::string& name() const;
// to use the same urdf somewhere else
const std::string& model_filename() const { return _model_filename; }
Expand Down Expand Up @@ -300,7 +287,6 @@ namespace robot_dart {
std::string _get_path(const std::string& filename) const;
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);

void _set_damages(const std::vector<RobotDamage>& damages);
void _set_color_mode(dart::dynamics::MeshShape::ColorMode color_mode, dart::dynamics::SkeletonPtr skel);
void _set_color_mode(dart::dynamics::MeshShape::ColorMode color_mode, dart::dynamics::ShapeNode* sn);
void _set_actuator_type(size_t joint_index, dart::dynamics::Joint::ActuatorType type, bool override_mimic = false, bool override_base = false);
Expand All @@ -317,7 +303,6 @@ namespace robot_dart {
std::string _model_filename;
std::vector<std::pair<std::string, std::string>> _packages;
dart::dynamics::SkeletonPtr _skeleton;
std::vector<RobotDamage> _damages;
std::vector<std::shared_ptr<control::RobotControl>> _controllers;
std::unordered_map<std::string, size_t> _dof_map, _joint_map;
bool _cast_shadows;
Expand Down
16 changes: 15 additions & 1 deletion src/tests/test_robot.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,8 +45,22 @@ BOOST_AUTO_TEST_CASE(test_constructors)
BOOST_CHECK(dummy->name() == "dummy_robot");
BOOST_CHECK(dummy->name() == dummy->skeleton()->getName());
BOOST_CHECK(dummy->name() == dummy_skel->getName());
}

BOOST_AUTO_TEST_CASE(test_dof_maps)
{
auto pendulum = std::make_shared<Robot>(std::string(ROBOT_DART_BUILD_DIR) + "/robots/pendulum.urdf");
BOOST_REQUIRE(pendulum);

// check dofs
auto names = pendulum->dof_names();
BOOST_CHECK(names.size() == 7);

// check if joint/dof map is updated
std::vector<std::string> name = {"pendulum_joint_1"};
pendulum->set_positions(make_vector({0.1}), name);

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

BOOST_AUTO_TEST_CASE(test_fix_free)
Expand Down