diff --git a/drake/multibody/rigid_body_tree.cc b/drake/multibody/rigid_body_tree.cc index 77a9869039c3..3d8bd25bdef3 100644 --- a/drake/multibody/rigid_body_tree.cc +++ b/drake/multibody/rigid_body_tree.cc @@ -129,8 +129,7 @@ RigidBodyTree::~RigidBodyTree(void) {} // rigid_body_tree.h. #ifndef SWIG template <> -unique_ptr> RigidBodyTree::Clone( - const std::vector& model_instance_ids) const { +unique_ptr> RigidBodyTree::Clone() const { auto clone = make_unique>(); clone->joint_limit_min = this->joint_limit_min; @@ -140,6 +139,7 @@ unique_ptr> RigidBodyTree::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) { @@ -201,16 +201,9 @@ unique_ptr> RigidBodyTree::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> 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& original_frame_body = original_frame->get_rigid_body(); @@ -251,17 +244,175 @@ template<> bool RigidBodyTree::CompareToClone(const RigidBodyTree& 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(this->loops.size()); ++i) { + const RigidBodyLoop* this_loop = &loops.at(i); + const RigidBodyLoop* 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::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& original_body = tree_->get_body(i); - // const RigidBody& 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; } diff --git a/drake/multibody/rigid_body_tree.h b/drake/multibody/rigid_body_tree.h index ad0bb5ce7630..b7ea3c206a65 100644 --- a/drake/multibody/rigid_body_tree.h +++ b/drake/multibody/rigid_body_tree.h @@ -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::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::Clone()" to + // drake-distro/drake/bindings/swig/rbtree.i does not work. #ifndef SWIG /** * Returns a deep clone of this RigidBodyTree. 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> Clone( - const std::vector& model_instance_ids = std::vector()) const; + std::unique_ptr> Clone() const; #endif /**