Skip to content

Commit

Permalink
Merge pull request #668 from ipab-slmc/wxm-fix-kinematictree-floating…
Browse files Browse the repository at this point in the history 
…-base-jacobian-fix

Fix floating-base Jacobian column switch bug, fix floating-base collision links
  • Loading branch information
@wxmerkt
wxmerkt committed Sep 22, 2019
2 parents 77419e2 + 95cf9ba commit 3fd535d
Show file tree
Hide file tree
Showing 17 changed files with 139 additions and 106 deletions.
5 changes: 1 addition & 4 deletions exotations/exotica_core_task_maps/include/exotica_core_task_maps/center_of_mass.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,9 +41,6 @@ namespace exotica
class CenterOfMass : public TaskMap, public Instantiable<CenterOfMassInitializer>
{
public:
CenterOfMass();
virtual ~CenterOfMass();

void AssignScene(ScenePtr scene) override;

void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi) override;
Expand All @@ -64,6 +61,6 @@ class CenterOfMass : public TaskMap, public Instantiable<CenterOfMassInitializer
bool enable_z_;
int dim_;
};
}
} // namespace exotica

#endif // EXOTICA_CORE_TASK_MAPS_COM_H_
3 changes: 2 additions & 1 deletion exotations/exotica_core_task_maps/include/exotica_core_task_maps/collision_distance.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,8 @@ class CollisionDistance : public TaskMap, public Instantiable<CollisionDistanceI
private:
void Initialize();

std::vector<std::string> robot_links_;
std::vector<std::string> robot_joints_;
std::map<std::string, std::vector<std::string>> controlled_joint_to_collision_link_map_;
bool check_self_collision_ = true;
double robot_margin_;
double world_margin_;
Expand Down
5 changes: 3 additions & 2 deletions exotations/exotica_core_task_maps/include/exotica_core_task_maps/smooth_collision_distance.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,8 @@ class SmoothCollisionDistance : public TaskMap, public Instantiable<SmoothCollis
private:
void Initialize();

std::vector<std::string> robot_links_;
std::vector<std::string> robot_joints_;
std::map<std::string, std::vector<std::string>> controlled_joint_to_collision_link_map_;
double robot_margin_ = 0.0;
double world_margin_ = 0.0;
bool linear_ = false;
Expand All @@ -63,6 +64,6 @@ class SmoothCollisionDistance : public TaskMap, public Instantiable<SmoothCollis

void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi, Eigen::MatrixXdRef J, bool updateJacobian = true);
};
}
} // namespace exotica

#endif // EXOTICA_CORE_TASK_MAPS_SMOOTH_COLLISION_DISTANCE_H_
16 changes: 5 additions & 11 deletions exotations/exotica_core_task_maps/src/center_of_mass.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,9 +34,6 @@ REGISTER_TASKMAP_TYPE("CenterOfMass", exotica::CenterOfMass);

namespace exotica
{
CenterOfMass::CenterOfMass() = default;
CenterOfMass::~CenterOfMass() = default;

void CenterOfMass::Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi)
{
if (phi.rows() != dim_) ThrowNamed("Wrong size of phi!");
Expand All @@ -58,7 +55,7 @@ void CenterOfMass::Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi)
com = com / M;
for (int i = 0; i < dim_; ++i) phi(i) = com[i];

if (debug_)
if (debug_ && Server::IsRos())
{
com_marker_.pose.position.x = phi(0);
com_marker_.pose.position.y = phi(1);
Expand Down Expand Up @@ -129,7 +126,7 @@ void CenterOfMass::Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi, Eig
}
for (int i = 0; i < dim_; ++i) phi(i) = com[i];

if (debug_)
if (debug_ && Server::IsRos())
{
com_marker_.pose.position.x = phi(0);
com_marker_.pose.position.y = phi(1);
Expand Down Expand Up @@ -174,11 +171,8 @@ void CenterOfMass::Initialize()
}
}

if (debug_)
{
InitializeDebug();
HIGHLIGHT_NAMED("CenterOfMass", "Total model mass: " << mass_.sum() << " kg");
}
if (Server::IsRos()) InitializeDebug();
if (debug_) HIGHLIGHT_NAMED("CenterOfMass", "Total model mass: " << mass_.sum() << " kg");
}

void CenterOfMass::AssignScene(ScenePtr scene)
Expand Down Expand Up @@ -213,4 +207,4 @@ void CenterOfMass::InitializeDebug()
com_links_pub_ = Server::Advertise<visualization_msgs::Marker>(object_name_ + "/com_links_marker", 1, true);
com_pub_ = Server::Advertise<visualization_msgs::Marker>(object_name_ + "/com_marker", 1, true);
}
}
} // namespace exotica
15 changes: 8 additions & 7 deletions exotations/exotica_core_task_maps/src/collision_distance.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,17 +51,17 @@ void CollisionDistance::Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi

void CollisionDistance::Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi, Eigen::MatrixXdRef J, bool updateJacobian)
{
cscene_->UpdateCollisionObjectTransforms();
if (!scene_->AlwaysUpdatesCollisionScene())
cscene_->UpdateCollisionObjectTransforms();

// For all robot links: Get all collision distances, sort by distance, and process the closest.
for (int i = 0; i < dim_; ++i)
{
// std::vector<CollisionProxy> proxies = cscene_->GetCollisionDistance(scene_->getControlledLinkToCollisionLinkMap()[robot_links_[i]], check_self_collision_); //, false);
std::vector<CollisionProxy> proxies = cscene_->GetCollisionDistance(robot_links_[i], check_self_collision_);
std::vector<CollisionProxy> proxies = cscene_->GetCollisionDistance(controlled_joint_to_collision_link_map_[robot_joints_[i]], check_self_collision_);
if (proxies.size() == 0)
{
phi(i) = 0;
J.row(i).setZero();
// phi(i) = 0;
// J.row(i).setZero();
continue;
}

Expand Down Expand Up @@ -111,8 +111,9 @@ void CollisionDistance::Initialize()
robot_margin_ = parameters_.RobotMargin;

// Get names of all controlled joints and their corresponding child links
robot_links_ = scene_->GetControlledLinkNames();
dim_ = static_cast<unsigned int>(robot_links_.size());
robot_joints_ = scene_->GetControlledJointNames();
controlled_joint_to_collision_link_map_ = scene_->GetControlledJointToCollisionLinkMap();
dim_ = static_cast<unsigned int>(robot_joints_.size());
closest_proxies_.assign(dim_, CollisionProxy());
if (debug_)
{
Expand Down
16 changes: 8 additions & 8 deletions exotations/exotica_core_task_maps/src/smooth_collision_distance.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ void SmoothCollisionDistance::Update(Eigen::VectorXdRefConst x,
{
if (phi.rows() != dim_) ThrowNamed("Wrong size of phi!");
phi.setZero();
Eigen::MatrixXd J(dim_, robot_links_.size());
Eigen::MatrixXd J(dim_, robot_joints_.size());
Update(x, phi, J, false);
}

Expand All @@ -62,11 +62,10 @@ void SmoothCollisionDistance::Update(Eigen::VectorXdRefConst x,

double& d = phi(0);

for (const auto& link : robot_links_)
for (const auto& joint : robot_joints_)
{
// Get all world collision links, then iterate through them
// std::vector<CollisionProxy> proxies = cscene_->GetCollisionDistance(scene_->getControlledLinkToCollisionLinkMap()[link], check_self_collision_);
std::vector<CollisionProxy> proxies = cscene_->GetCollisionDistance(link, check_self_collision_);
std::vector<CollisionProxy> proxies = cscene_->GetCollisionDistance(controlled_joint_to_collision_link_map_[joint], check_self_collision_);

for (const auto& proxy : proxies)
{
Expand Down Expand Up @@ -126,12 +125,13 @@ void SmoothCollisionDistance::Initialize()

if (robot_margin_ == 0.0 || world_margin_ == 0.0) ThrowPretty("Setting the margin to zero is a bad idea. It will NaN.");

HIGHLIGHT_NAMED("Smooth Collision Distance",
"World Margin: " << world_margin_ << " Robot Margin: " << robot_margin_ << "\t Linear: " << linear_);
if (debug_) HIGHLIGHT_NAMED("Smooth Collision Distance",
"World Margin: " << world_margin_ << " Robot Margin: " << robot_margin_ << "\t Linear: " << linear_);

// Get names of all controlled joints and their corresponding child links
robot_links_ = scene_->GetControlledLinkNames();
robot_joints_ = scene_->GetControlledJointNames();
controlled_joint_to_collision_link_map_ = scene_->GetControlledJointToCollisionLinkMap();
}

int SmoothCollisionDistance::TaskSpaceDim() { return dim_; }
}
} // namespace exotica
11 changes: 5 additions & 6 deletions exotica_core/include/exotica_core/kinematic_tree.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -184,14 +184,13 @@ class KinematicTree : public Uncopyable
Eigen::MatrixXd Jdot(const KDL::Jacobian& jacobian);

void ResetModel();
std::shared_ptr<KinematicElement> AddElement(const std::string& name, Eigen::Isometry3d& transform, const std::string& parent = "", shapes::ShapeConstPtr shape = shapes::ShapeConstPtr(nullptr), const KDL::RigidBodyInertia& inertia = KDL::RigidBodyInertia::Zero(), const Eigen::Vector4d& color = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0), const std::vector<VisualElement>& visual = {}, bool is_controlled = false);
std::shared_ptr<KinematicElement> AddEnvironmentElement(const std::string& name, Eigen::Isometry3d& transform, const std::string& parent = "", shapes::ShapeConstPtr shape = shapes::ShapeConstPtr(nullptr), const KDL::RigidBodyInertia& inertia = KDL::RigidBodyInertia::Zero(), const Eigen::Vector4d& color = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0), const std::vector<VisualElement>& visual = {}, bool is_controlled = false);
std::shared_ptr<KinematicElement> AddElement(const std::string& name, Eigen::Isometry3d& transform, const std::string& parent, const std::string& shape_resource_path, Eigen::Vector3d scale = Eigen::Vector3d::Ones(), const KDL::RigidBodyInertia& inertia = KDL::RigidBodyInertia::Zero(), const Eigen::Vector4d& color = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0), const std::vector<VisualElement>& visual = {}, bool is_controlled = false);
std::shared_ptr<KinematicElement> AddElement(const std::string& name, const Eigen::Isometry3d& transform, const std::string& parent = "", shapes::ShapeConstPtr shape = shapes::ShapeConstPtr(nullptr), const KDL::RigidBodyInertia& inertia = KDL::RigidBodyInertia::Zero(), const Eigen::Vector4d& color = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0), const std::vector<VisualElement>& visual = {}, bool is_controlled = false);
std::shared_ptr<KinematicElement> AddEnvironmentElement(const std::string& name, const Eigen::Isometry3d& transform, const std::string& parent = "", shapes::ShapeConstPtr shape = shapes::ShapeConstPtr(nullptr), const KDL::RigidBodyInertia& inertia = KDL::RigidBodyInertia::Zero(), const Eigen::Vector4d& color = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0), const std::vector<VisualElement>& visual = {}, bool is_controlled = false);
std::shared_ptr<KinematicElement> AddElement(const std::string& name, const Eigen::Isometry3d& transform, const std::string& parent, const std::string& shape_resource_path, Eigen::Vector3d scale = Eigen::Vector3d::Ones(), const KDL::RigidBodyInertia& inertia = KDL::RigidBodyInertia::Zero(), const Eigen::Vector4d& color = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0), const std::vector<VisualElement>& visual = {}, bool is_controlled = false);
void UpdateModel();
void ChangeParent(const std::string& name, const std::string& parent, const KDL::Frame& pose, bool relative);
int IsControlled(std::shared_ptr<KinematicElement> joint);
int IsControlled(std::string joint_name);
int IsControlledLink(std::string link_name);
int IsControlledLink(const std::string& link_name);

Eigen::VectorXd GetModelState() const;
std::map<std::string, double> GetModelStateMap() const;
Expand Down Expand Up @@ -235,7 +234,7 @@ class KinematicTree : public Uncopyable

private:
void BuildTree(const KDL::Tree& RobotKinematics);
void AddElement(KDL::SegmentMap::const_iterator segment, std::shared_ptr<KinematicElement> parent);
void AddElementFromSegmentMapIterator(KDL::SegmentMap::const_iterator segment, std::shared_ptr<KinematicElement> parent);
void UpdateTree();
void UpdateFK();
void UpdateJ();
Expand Down
16 changes: 8 additions & 8 deletions exotica_core/include/exotica_core/scene.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -165,15 +165,15 @@ class Scene : public Object, Uncopyable, public Instantiable<SceneInitializer>,
bool AlwaysUpdatesCollisionScene() const { return force_collision_; }
/// @brief Returns a map between a model link name and the names of associated collision links.
/// @return Map between model links and all associated collision links.
std::map<std::string, std::vector<std::string>> GetModelLinkToCollisionLinkMap() const { return model_link_to_collision_link_map_; };
const std::map<std::string, std::vector<std::string>>& GetModelLinkToCollisionLinkMap() const { return model_link_to_collision_link_map_; };
/// @brief Returns a map between a model link name and the KinematicElement of associated collision links.
/// @return Map between model links and all the KinematicElements of the associated collision links.
std::map<std::string, std::vector<std::shared_ptr<KinematicElement>>> GetModelLinkToCollisionElementMap() const { return model_link_to_collision_element_map_; };
/// @brief Returns a map between controlled robot link names and associated collision link names. Here we consider all fixed links between controlled links as belonging to the previous controlled link (as if the collision links had been fused).
/// @return Map between controlled links and associated collision links.
std::map<std::string, std::vector<std::string>> GetControlledLinkToCollisionLinkMap() const { return controlled_link_to_collision_link_map_; };
const std::map<std::string, std::vector<std::shared_ptr<KinematicElement>>>& GetModelLinkToCollisionElementMap() const { return model_link_to_collision_element_map_; };
/// @brief Returns a map between controlled robot joint names and associated collision link names. Here we consider all fixed links between controlled links as belonging to the previous controlled joint (as if the collision links had been fused).
/// @return Map between controlled joints and associated collision links.
const std::map<std::string, std::vector<std::string>>& GetControlledJointToCollisionLinkMap() const { return controlled_joint_to_collision_link_map_; };
/// @brief Returns world links that are to be excluded from collision checking.
std::set<std::string> get_world_links_to_exclude_from_collision_scene() const { return world_links_to_exclude_from_collision_scene_; }
const std::set<std::string>& get_world_links_to_exclude_from_collision_scene() const { return world_links_to_exclude_from_collision_scene_; }
private:
void UpdateInternalFrames(bool update_request = true);

Expand Down Expand Up @@ -213,8 +213,8 @@ class Scene : public Object, Uncopyable, public Instantiable<SceneInitializer>,
std::map<std::string, std::vector<std::string>> model_link_to_collision_link_map_;
std::map<std::string, std::vector<std::shared_ptr<KinematicElement>>> model_link_to_collision_element_map_;

/// \brief Mapping between controlled link name and collision links
std::map<std::string, std::vector<std::string>> controlled_link_to_collision_link_map_;
/// \brief Mapping between controlled joint name and collision links
std::map<std::string, std::vector<std::string>> controlled_joint_to_collision_link_map_;

/// \brief List of robot links to be excluded from the collision scene
std::set<std::string> robot_links_to_exclude_from_collision_scene_;
Expand Down
2 changes: 1 addition & 1 deletion exotica_core/include/exotica_core/visualization_moveit.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,6 @@ class VisualizationMoveIt : public Uncopyable
ScenePtr scene_ = std::make_shared<Scene>(nullptr);
ros::Publisher trajectory_pub_;
};
}
} // namespace exotica

#endif // EXOTICA_CORE_VISUALIZATION_MOVEIT_H_
48 changes: 30 additions & 18 deletions exotica_core/src/kinematic_tree.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -190,6 +190,9 @@ void KinematicTree::BuildTree(const KDL::Tree& robot_kinematics)
if (RotW != controlled_joints_names_.end()) controlled_joints_names_.erase(RotW);
RotW = std::find(model_joints_names_.begin(), model_joints_names_.end(), root_joint->getVariableNames()[6]);
if (RotW != model_joints_names_.end()) model_joints_names_.erase(RotW);

// NB: We do not want to swap the XYZ labels for the rotation joints
// to not change the order in which joint and model state are stored.
}
else if (root_joint->getType() == robot_model::JointModel::PLANAR)
{
Expand All @@ -212,7 +215,7 @@ void KinematicTree::BuildTree(const KDL::Tree& robot_kinematics)
ThrowPretty("Unsupported root joint type: " << root_joint->getTypeName());
}

AddElement(robot_kinematics.getRootSegment(), *(model_tree_.end() - 1));
AddElementFromSegmentMapIterator(robot_kinematics.getRootSegment(), *(model_tree_.end() - 1));

// Set root inertial
if (root_joint->getType() == robot_model::JointModel::FIXED)
Expand Down Expand Up @@ -425,14 +428,14 @@ void KinematicTree::ChangeParent(const std::string& name, const std::string& par
debug_scene_changed_ = true;
}

std::shared_ptr<KinematicElement> KinematicTree::AddEnvironmentElement(const std::string& name, Eigen::Isometry3d& transform, const std::string& parent, shapes::ShapeConstPtr shape, const KDL::RigidBodyInertia& inertia, const Eigen::Vector4d& color, const std::vector<VisualElement>& visual, bool is_controlled)
std::shared_ptr<KinematicElement> KinematicTree::AddEnvironmentElement(const std::string& name, const Eigen::Isometry3d& transform, const std::string& parent, shapes::ShapeConstPtr shape, const KDL::RigidBodyInertia& inertia, const Eigen::Vector4d& color, const std::vector<VisualElement>& visual, bool is_controlled)
{
std::shared_ptr<KinematicElement> element = AddElement(name, transform, parent, shape, inertia, color, visual, is_controlled);
environment_tree_.push_back(element);
return element;
}

std::shared_ptr<KinematicElement> KinematicTree::AddElement(const std::string& name, Eigen::Isometry3d& transform, const std::string& parent, const std::string& shape_resource_path, Eigen::Vector3d scale, const KDL::RigidBodyInertia& inertia, const Eigen::Vector4d& color, const std::vector<VisualElement>& visual, bool is_controlled)
std::shared_ptr<KinematicElement> KinematicTree::AddElement(const std::string& name, const Eigen::Isometry3d& transform, const std::string& parent, const std::string& shape_resource_path, Eigen::Vector3d scale, const KDL::RigidBodyInertia& inertia, const Eigen::Vector4d& color, const std::vector<VisualElement>& visual, bool is_controlled)
{
std::string shape_path(shape_resource_path);
if (shape_path.empty())
Expand Down Expand Up @@ -460,7 +463,7 @@ std::shared_ptr<KinematicElement> KinematicTree::AddElement(const std::string& n
return element;
}

std::shared_ptr<KinematicElement> KinematicTree::AddElement(const std::string& name, Eigen::Isometry3d& transform, const std::string& parent, shapes::ShapeConstPtr shape, const KDL::RigidBodyInertia& inertia, const Eigen::Vector4d& color, const std::vector<VisualElement>& visual, bool is_controlled)
std::shared_ptr<KinematicElement> KinematicTree::AddElement(const std::string& name, const Eigen::Isometry3d& transform, const std::string& parent, shapes::ShapeConstPtr shape, const KDL::RigidBodyInertia& inertia, const Eigen::Vector4d& color, const std::vector<VisualElement>& visual, bool is_controlled)
{
std::shared_ptr<KinematicElement> parent_element;
if (parent == "")
Expand Down Expand Up @@ -503,14 +506,14 @@ std::shared_ptr<KinematicElement> KinematicTree::AddElement(const std::string& n
return new_element;
}

void KinematicTree::AddElement(KDL::SegmentMap::const_iterator segment, std::shared_ptr<KinematicElement> parent)
void KinematicTree::AddElementFromSegmentMapIterator(KDL::SegmentMap::const_iterator segment, std::shared_ptr<KinematicElement> parent)
{
std::shared_ptr<KinematicElement> new_element = std::make_shared<KinematicElement>(model_tree_.size(), parent, segment->second.segment);
model_tree_.push_back(new_element);
if (parent) parent->children.push_back(new_element);
for (KDL::SegmentMap::const_iterator child : segment->second.children)
{
AddElement(child, new_element);
AddElementFromSegmentMapIterator(child, new_element);
}
}

Expand All @@ -523,21 +526,30 @@ int KinematicTree::IsControlled(std::shared_ptr<KinematicElement> joint)
return -1;
}

int KinematicTree::IsControlled(std::string joint_name)
int KinematicTree::IsControlledLink(const std::string& link_name)
{
for (int i = 0; i < controlled_joints_names_.size(); ++i)
try
{
if (controlled_joints_names_[i] == joint_name) return i;
}
return -1;
}
auto element = tree_map_[link_name].lock();

int KinematicTree::IsControlledLink(std::string link_name)
{
for (int i = 0; i < controlled_joints_.size(); ++i)
if (element && element->is_controlled)
{
return element->control_id;
}

while (element)
{
element = element->parent.lock();

if (element && element->is_controlled)
{
return element->control_id;
}
}
}
catch (const std::out_of_range& e)
{
auto joint = controlled_joints_[i].lock();
if (joint->segment.getName() == link_name) return i;
return -1;
}
return -1;
}
Expand Down Expand Up @@ -1188,4 +1200,4 @@ bool KinematicTree::DoesLinkWithNameExist(std::string name) const
// Check whether it exists in TreeMap, which should encompass both EnvironmentTree and model_tree_
return tree_map_.find(name) != tree_map_.end();
}
}
} // namespace exotica
Loading

0 comments on commit 3fd535d

Please sign in to comment.