Skip to content

Commit

Permalink
WIP
Browse files Browse the repository at this point in the history
  • Loading branch information
Chien-Liang Fok committed Feb 5, 2017
1 parent 6c656b6 commit 1948071
Show file tree
Hide file tree
Showing 2 changed files with 178 additions and 27 deletions.
187 changes: 169 additions & 18 deletions drake/multibody/rigid_body_tree.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -129,8 +129,7 @@ RigidBodyTree<T>::~RigidBodyTree(void) {}
// rigid_body_tree.h.
#ifndef SWIG
template <>
unique_ptr<RigidBodyTree<double>> RigidBodyTree<double>::Clone(
const std::vector<int>& model_instance_ids) const {
unique_ptr<RigidBodyTree<double>> RigidBodyTree<double>::Clone() const {
auto clone = make_unique<RigidBodyTree<double>>();

clone->joint_limit_min = this->joint_limit_min;
Expand All @@ -140,6 +139,7 @@ unique_ptr<RigidBodyTree<double>> RigidBodyTree<double>::Clone(
clone->num_positions_ = this->num_positions_;
clone->num_velocities_ = this->num_velocities_;
clone->num_model_instances_ = this->num_model_instances_;
clone->initialized_ = this->initialized_;

// Clones the rigid bodies.
for (const auto& body : bodies) {
Expand Down Expand Up @@ -201,16 +201,9 @@ unique_ptr<RigidBodyTree<double>> RigidBodyTree<double>::Clone(

// Adds joints to the cloned RigidBodyFrame objects.
for (const auto& original_frame : frames) {
std::cout << "original frame name: " << original_frame->get_name()
<< ", model instance id = "
<< original_frame->get_model_instance_id()
<< std::endl;
std::shared_ptr<RigidBodyFrame<double>> cloned_frame =
clone->findFrame(original_frame->get_name(),
original_frame->get_model_instance_id());
std::cout << "Obtained cloned frame named: " << cloned_frame->get_name()
<< ", model instance id = "
<< cloned_frame->get_model_instance_id() << std::endl;
DRAKE_DEMAND(cloned_frame != nullptr);
const RigidBody<double>& original_frame_body =
original_frame->get_rigid_body();
Expand Down Expand Up @@ -251,17 +244,175 @@ template<>
bool RigidBodyTree<double>::CompareToClone(const RigidBodyTree<double>& other)
const {
if (this->get_num_model_instances() != other.get_num_model_instances()) {
drake::log()->debug(
"RigidBodyTree::CompareToClone(): num model instances mismatch:\n"
" - this: {}\n"
" - other: {}",
this->get_num_model_instances(),
other.get_num_model_instances());
return false;
}
if (this->get_num_bodies() != other.get_num_bodies()) {
drake::log()->debug(
"RigidBodyTree::CompareToClone(): num bodies mismatch:\n"
" - this: {}\n"
" - other: {}",
this->get_num_bodies(),
other.get_num_bodies());
return false;
}
if (this->get_num_frames() != other.get_num_frames()) {
drake::log()->debug(
"RigidBodyTree::CompareToClone(): num bodies mismatch:\n"
" - this: {}\n"
" - other: {}",
this->get_num_frames(),
other.get_num_frames());
return false;
}
if (this->get_num_positions() != other.get_num_positions()) {
drake::log()->debug(
"RigidBodyTree::CompareToClone(): num positions mismatch:\n"
" - this: {}\n"
" - other: {}",
this->get_num_positions(),
other.get_num_positions());
return false;
}
if (this->get_num_velocities() != other.get_num_velocities()) {
drake::log()->debug(
"RigidBodyTree::CompareToClone(): num velocities mismatch:\n"
" - this: {}\n"
" - other: {}",
this->get_num_velocities(),
other.get_num_velocities());
return false;
}
if (this->get_num_actuators() != other.get_num_actuators()) {
drake::log()->debug(
"RigidBodyTree::CompareToClone(): num velocities mismatch:\n"
" - this: {}\n"
" - other: {}",
this->get_num_actuators(),
other.get_num_actuators());
return false;
}
for (int i = 0; i < this->get_num_bodies(); ++i) {
if (!this->bodies.at(i)->CompareToClone(*other.bodies.at(i))) {
drake::log()->debug(
"RigidBodyTree::CompareToClone(): bodies mismatch at index {}.", i);
return false;
}
}
for (int i = 0; i < this->get_num_frames(); ++i) {
if (!this->frames.at(i)->CompareToClone(*other.frames.at(i))) {
drake::log()->debug(
"RigidBodyTree::CompareToClone(): frames mismatch at index {}.", i);
return false;
}
}
for (int i = 0; i < this->get_num_actuators(); ++i) {
const RigidBodyActuator* this_actuator = &actuators.at(i);
const RigidBodyActuator* other_actuator = &other.actuators.at(i);
if (this_actuator->name_ != other_actuator->name_) {
drake::log()->debug(
"RigidBodyTree::CompareToClone(): actuators mismatch at index {}. "
"Names do not match:\n"
" - this: {}\n"
" - other: {}",
i, this_actuator->name_, other_actuator->name_);
return false;
}
if (!this_actuator->body_->CompareToClone(*other_actuator->body_)) {
drake::log()->debug(
"RigidBodyTree::CompareToClone(): actuator mismatch at index {}. "
"Bodies do not match.", i);
return false;
}
if (this_actuator->reduction_ != other_actuator->reduction_) {
drake::log()->debug(
"RigidBodyTree::CompareToClone(): actuator mismatch at index {}. "
"Reductions do not match:\n"
" - this: {}\n"
" - other: {}",
i, this_actuator->reduction_, other_actuator->reduction_);
return false;
}
if (this_actuator->effort_limit_min_ != other_actuator->effort_limit_min_) {
drake::log()->debug(
"RigidBodyTree::CompareToClone(): actuator mismatch at index {}. "
"Minimum effort limits do not match:\n"
" - this: {}\n"
" - other: {}",
i, this_actuator->effort_limit_min_,
other_actuator->effort_limit_min_);
return false;
}
if (this_actuator->effort_limit_max_ != other_actuator->effort_limit_max_) {
drake::log()->debug(
"RigidBodyTree::CompareToClone(): actuator mismatch at index {}. "
"Maximum effort limits do not match:\n"
" - this: {}\n"
" - other: {}",
i, this_actuator->effort_limit_max_,
other_actuator->effort_limit_max_);
return false;
}
}
for (int i = 0; i < static_cast<int>(this->loops.size()); ++i) {
const RigidBodyLoop<double>* this_loop = &loops.at(i);
const RigidBodyLoop<double>* other_loop = &other.loops.at(i);
if (!this_loop->frameA_->CompareToClone(*other_loop->frameA_)) {
drake::log()->debug(
"RigidBodyTree::CompareToClone(): loop mismatch at index {}. "
"FrameAs do not match.", i);
return false;
}
if (!this_loop->frameB_->CompareToClone(*other_loop->frameB_)) {
drake::log()->debug(
"RigidBodyTree::CompareToClone(): loop mismatch at index {}. "
"FrameBs do not match.", i);
return false;
}
if (!this_loop->axis_.isApprox(other_loop->axis_,
std::numeric_limits<double>::epsilon())) {
drake::log()->debug(
"RigidBodyTree::CompareToClone(): loop mismatch at index {}. "
"Axes do not match:\n"
" - this: {}\n"
" - other: {}",
i, this_loop->axis_.transpose(),
other_loop->axis_.transpose());
return false;
}
}
if (this->a_grav != other.a_grav) {
drake::log()->debug(
"RigidBodyTree::CompareToClone(): gravity vector mismatch:\n"
" - this: {}\n"
" - other: {}",
this->a_grav,
other.a_grav);
return false;
}
if (this->B != other.B) {
drake::log()->debug(
"RigidBodyTree::CompareToClone(): B matrix mismatch:\n"
" - this:\n{}\n"
" - other:\n{}",
this->B,
other.B);
return false;
}
if (this->initialized_ != other.initialized_) {
drake::log()->debug(
"RigidBodyTree::CompareToClone(): initialized_ mismatch:\n"
" - this:\n{}\n"
" - other:\n{}",
this->initialized_,
other.initialized_);
return false;
}

// for (int i = 0; i < tree_->get_num_bodies(); ++i) {
// const RigidBody<double>& original_body = tree_->get_body(i);
// const RigidBody<double>& cloned_body = cloned_tree->get_body(i);
// EXPECT_EQ(original_body.get_name(), cloned_body.get_name());
// EXPECT_EQ(original_body.get_model_name(), cloned_body.get_model_name());
// EXPECT_EQ(original_body.get_model_instance_id(),
// cloned_body.get_model_instance_id());
// }
return true;
}

Expand Down
18 changes: 9 additions & 9 deletions drake/multibody/rigid_body_tree.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,23 +113,23 @@ class RigidBodyTree {

virtual ~RigidBodyTree(void);

// TODO(liang.fok) The following preprocessor condition is necessary because
// wrapping method Clone() in SWIG caused a build error to the effect of "call
// to implicitly-deleted copy constructor". Unfortunately, adding
// "%ignore RigidBodyTree<double>::Clone()" to
// drake-distro/drake/bindings/swig/rbtree.i does not work for some unknown
// reason.
// The following preprocessor condition is necessary because wrapping method
// Clone() in SWIG causes the following build error to occur:
//
// "call to implicitly-deleted copy constructor"
//
// Unfortunately, adding "%ignore RigidBodyTree<double>::Clone()" to
// drake-distro/drake/bindings/swig/rbtree.i does not work.
#ifndef SWIG
/**
* Returns a deep clone of this RigidBodyTree<double>. Currently, everything
* *except* for collision elements are cloned.
* *except* for collision and visual elements are cloned.
*
* @param[in] model_instance_ids A set of model instance IDs of the model
* instances to be included in the returned clone. By default, all model
* instances are included.
*/
std::unique_ptr<RigidBodyTree<double>> Clone(
const std::vector<int>& model_instance_ids = std::vector<int>()) const;
std::unique_ptr<RigidBodyTree<double>> Clone() const;
#endif

/**
Expand Down

0 comments on commit 1948071

Please sign in to comment.