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

Allow to "break" sensors #153

Merged
merged 6 commits into from
Oct 1, 2021
Merged

Allow to "break" sensors #153

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
e41d9b2
detach a sensor
jbmouret Sep 20, 2021
8e89871
allow to detach a sensor
jbmouret Sep 21, 2021
0f7b619
added constraint_forces for spd computation with damage
dalinel Sep 30, 2021
d609d95
remove some mac stuffs
jbmouret Sep 30, 2021
dfe6fad
Merge branch 'break_sensors' of https://github.com/resibots/robot_dar…
jbmouret Sep 30, 2021
188ee84
fix minor issues (nullptr, assert)
jbmouret Oct 1, 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
11 changes: 10 additions & 1 deletion src/robot_dart/robot.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,8 @@ namespace robot_dart {
return skeleton->getGravityForces();
else if (content == 15)
return skeleton->getCoriolisAndGravityForces();
else if (content == 16)
return skeleton->getConstraintForces();
ROBOT_DART_EXCEPTION_ASSERT(false, "Unknown type of data!");
}

Expand All @@ -81,6 +83,8 @@ namespace robot_dart {
tmp = skeleton->getGravityForces();
else if (content == 15)
tmp = skeleton->getCoriolisAndGravityForces();
else if (content == 16)
tmp = skeleton->getConstraintForces();

for (size_t i = 0; i < dof_names.size(); i++) {
auto it = dof_map.find(dof_names[i]);
Expand Down Expand Up @@ -112,7 +116,7 @@ namespace robot_dart {
data(i) = dof->getForceLowerLimit();
else if (content == 12)
data(i) = dof->getForceUpperLimit();
else if (content == 13 || content == 14 || content == 15)
else if (content == 13 || content == 14 || content == 15 || content == 16)
data(i) = tmp(it->second);
else
ROBOT_DART_EXCEPTION_ASSERT(false, "Unknown type of data!");
Expand Down Expand Up @@ -1618,6 +1622,11 @@ namespace robot_dart {
return detail::dof_data<15>(_skeleton, dof_names, _dof_map);
}

Eigen::VectorXd Robot::constraint_forces(const std::vector<std::string>& dof_names) const
{
return detail::dof_data<16>(_skeleton, dof_names, _dof_map);
}

Eigen::VectorXd Robot::vec_dof(const Eigen::VectorXd& vec, const std::vector<std::string>& dof_names) const
{
assert(vec.size() == static_cast<int>(_skeleton->getNumDofs()));
Expand Down
1 change: 1 addition & 0 deletions src/robot_dart/robot.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -231,6 +231,7 @@ namespace robot_dart {
Eigen::VectorXd coriolis_forces(const std::vector<std::string>& dof_names = {}) const;
Eigen::VectorXd gravity_forces(const std::vector<std::string>& dof_names = {}) const;
Eigen::VectorXd coriolis_gravity_forces(const std::vector<std::string>& dof_names = {}) const;
Eigen::VectorXd constraint_forces(const std::vector<std::string>& dof_names = {}) const;

// Get only the part of vector for DOFs in dof_names
Eigen::VectorXd vec_dof(const Eigen::VectorXd& vec, const std::vector<std::string>& dof_names) const;
Expand Down
19 changes: 18 additions & 1 deletion src/robot_dart/sensor/sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,8 +43,18 @@ namespace robot_dart {
void Sensor::set_pose(const Eigen::Isometry3d& tf) { _world_pose = tf; }
const Eigen::Isometry3d& Sensor::pose() const { return _world_pose; }

void Sensor::detach() {
_attached_to_body = false;
_attached_to_joint = false;
_body_attached = NULL;
_joint_attached = NULL;
_active = false;
}

void Sensor::refresh(double t)
{
if (!_active)
return;
if (_attaching_to_body && !_attached_to_body) {
attach_to_body(_body_attached, _attached_tf);
}
Expand All @@ -71,7 +81,6 @@ namespace robot_dart {
if (body)
_world_pose = body->getWorldTransform() * tf * _attached_tf;
}

calculate(t);
}

Expand Down Expand Up @@ -111,5 +120,13 @@ namespace robot_dart {
_attached_to_joint = false;
}
}
const std::string& Sensor::attached_to() const
{
assert(_attached_to_body || _attached_to_joint);
if (_attached_to_body)
return _body_attached->getName();
// attached to joint
return _joint_attached->getName();
}
} // namespace sensor
} // namespace robot_dart
2 changes: 2 additions & 0 deletions src/robot_dart/sensor/sensor.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,8 @@ namespace robot_dart {
virtual void attach_to_joint(dart::dynamics::Joint* joint, const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity());
void attach_to_joint(const std::shared_ptr<Robot>& robot, const std::string& joint_name, const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity()) { attach_to_joint(robot->joint(joint_name), tf); }

void detach();
const std::string& attached_to() const;
protected:
RobotDARTSimu* _simu = nullptr;
bool _active;
Expand Down