New EffBox task map.

exotations/exotica_core_task_maps/CMakeLists.txt

@@ -17,7 +17,9 @@ AddInitializer(
  eff_orientation
  eff_frame
  frame_with_axis_and_direction
+ frame_with_box_limits
  eff_axis_alignment
+ eff_box
  eff_velocity
  gaze_at_constraint
  joint_limit
@@ -59,6 +61,7 @@ set(SOURCES
  src/eff_orientation.cpp
  src/eff_frame.cpp
  src/eff_axis_alignment.cpp
+ src/eff_box.cpp
  src/eff_velocity.cpp
  src/gaze_at_constraint.cpp
  src/joint_limit.cpp

exotations/exotica_core_task_maps/exotica_plugins.xml

@@ -1,3 +1,4 @@
+<?xml version="1.0" encoding="utf-8"?>
 <library path="lib/libexotica_core_task_maps">
  <class name="exotica/ContinuousJointPose" type="exotica::ContinuousJointPose" base_class_type="exotica::TaskMap">
  <description>Representation of the SO(2) group using cos/sin</description>
@@ -76,4 +77,7 @@
  Avoids "looking at" spherical objects. Can be used as either a cost term, or inequality constraint. The task map assumes the positions and radii of each object. 
  </description>
  </class> 
+ <class name="exotica/EffBox" type="exotica::EffBox" base_class_type="exotica::TaskMap">
+ <description>Limits the end-effector motion to a box in some reference frame. This task map can only be used as an inequality constraint.</description>
+ </class>
 </library>

exotations/exotica_core_task_maps/include/exotica_core_task_maps/eff_box.h

@@ -0,0 +1,67 @@
+//
+// Copyright (c) 2019, Christopher E. Mower
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of nor the names of its contributors may be used to
+// endorse or promote products derived from this software without specific
+// prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+
+#ifndef EXOTICA_CORE_TASK_MAPS_EFF_BOX_H_
+#define EXOTICA_CORE_TASK_MAPS_EFF_BOX_H_
+
+#include <exotica_core/task_map.h>
+
+#include <exotica_core_task_maps/eff_box_initializer.h>
+#include <exotica_core_task_maps/frame_with_box_limits_initializer.h>
+
+namespace exotica
+{
+/// \class EffBox
+///
+/// \ingroup TaskMap
+///
+/// \brief Limits every given end-effector motion to a box in some reference frame.
+class EffBox : public TaskMap, public Instantiable<EffBoxInitializer>
+{
+public:
+ void Instantiate(const EffBoxInitializer& init) override; // TODO: Allow user to use task map as both a cost term and inequality constraint
+ void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi) override;
+ void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi, Eigen::MatrixXdRef jacobian) override;
+ int TaskSpaceDim() override;
+
+ void PublishObjectsAsMarkerArray();
+
+ Eigen::VectorXd GetLowerLimit() const;
+ Eigen::VectorXd GetUpperLimit() const;
+
+private:
+ Eigen::VectorXd eff_lower_; ///< End-effector lower x, y, z limit.
+ Eigen::VectorXd eff_upper_; ///< End-effector upper x, y, z limit.
+ int n_effs_; ///< Number of end-effectors.
+ int three_times_n_effs_; ///> Three multiplied by the number of end-effectors.
+ ros::Publisher pub_markers_; ///< publish marker for RViz
+};
+} // namespace exotica
+
+#endif // EXOTICA_CORE_TASK_MAPS_EFF_BOX_H_

exotations/exotica_core_task_maps/init/eff_box.in

@@ -0,0 +1,10 @@
+class EffBox
+
+extend <exotica_core/task_map>
+
+// inherited from TaskMap:
+// string Link > name of frame in which point is defined
+// VectorXd LinkOffset > coordinate of point in Link frame
+// string Base > name of frame in which line is defined
+// VectorXd BaseOffset > starting point of line in Base frame
+

exotations/exotica_core_task_maps/init/frame_with_box_limits.in

@@ -0,0 +1,7 @@
+class FrameWithBoxLimits
+
+extend <exotica_core/frame>
+
+Required Eigen::Vector2d XLim;
+Required Eigen::Vector2d YLim;
+Required Eigen::Vector2d ZLim;

exotations/exotica_core_task_maps/src/eff_box.cpp

@@ -0,0 +1,167 @@
+//
+// Copyright (c) 2019, Christopher E. Mower
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of nor the names of its contributors may be used to
+// endorse or promote products derived from this software without specific
+// prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+
+#include <exotica_core/server.h>
+#include <exotica_core_task_maps/eff_box.h>
+
+REGISTER_TASKMAP_TYPE("EffBox", exotica::EffBox);
+
+namespace exotica
+{
+void EffBox::Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi)
+{
+ if (phi.rows() != TaskSpaceDim()) ThrowNamed("Wrong size of phi!");
+
+ for (int i = 0; i < n_effs_; ++i)
+ {
+ // Setup
+ const int eff_id = 3 * i;
+
+ // Compute phi
+ phi.segment(eff_id, 3) = Eigen::Map<Eigen::Vector3d>(kinematics[0].Phi(i).p.data) - eff_upper_.segment<3>(eff_id);
+ phi.segment(eff_id + three_times_n_effs_, 3) = eff_lower_.segment<3>(eff_id) - Eigen::Map<Eigen::Vector3d>(kinematics[0].Phi(i).p.data);
+ }
+
+ if (debug_ && Server::IsRos()) PublishObjectsAsMarkerArray();
+}
+
+void EffBox::Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi, Eigen::MatrixXdRef jacobian)
+{
+ if (phi.rows() != TaskSpaceDim()) ThrowNamed("Wrong size of phi!");
+ if (jacobian.rows() != TaskSpaceDim() || jacobian.cols() != kinematics[0].jacobian(0).data.cols()) ThrowNamed("Wrong size of jacobian! " << kinematics[0].jacobian(0).data.cols());
+
+ for (int i = 0; i < n_effs_; ++i)
+ {
+ // Setup
+ const int eff_id = 3 * i;
+
+ // Compute phi and jacobian
+ phi.segment(eff_id, 3) = Eigen::Map<Eigen::Vector3d>(kinematics[0].Phi(i).p.data) - eff_upper_.segment<3>(eff_id);
+ phi.segment(eff_id + three_times_n_effs_, 3) = eff_lower_.segment<3>(eff_id) - Eigen::Map<Eigen::Vector3d>(kinematics[0].Phi(i).p.data);
+ jacobian.middleRows(eff_id, 3) = kinematics[0].jacobian(i).data.topRows<3>();
+ jacobian.middleRows(eff_id + three_times_n_effs_, 3) = -kinematics[0].jacobian(i).data.topRows<3>();
+ }
+
+ if (debug_ && Server::IsRos()) PublishObjectsAsMarkerArray();
+}
+
+Eigen::VectorXd EffBox::GetLowerLimit() const
+{
+ return eff_lower_;
+}
+
+Eigen::VectorXd EffBox::GetUpperLimit() const
+{
+ return eff_upper_;
+}
+
+void EffBox::PublishObjectsAsMarkerArray()
+{
+ const ros::Time t = ros::Time::now();
+ visualization_msgs::MarkerArray ma;
+ ma.markers.reserve(n_effs_);
+ for (int i = 0; i < n_effs_; ++i)
+ {
+ const int eff_id = 3 * i;
+ visualization_msgs::Marker m;
+ m.header.stamp = t;
+ std::string frame_name;
+ if (frames_[i].frame_B_link_name == "")
+ {
+ frame_name = scene_->GetRootFrameName();
+ }
+ else
+ {
+ frame_name = frames_[i].frame_B_link_name;
+ }
+ m.header.frame_id = "exotica/" + frame_name;
+ m.id = i;
+ m.action = visualization_msgs::Marker::ADD;
+ m.type = visualization_msgs::Marker::CUBE;
+ m.scale.x = eff_upper_(eff_id) - eff_lower_(eff_id);
+ m.scale.y = eff_upper_(eff_id + 1) - eff_lower_(eff_id + 1);
+ m.scale.z = eff_upper_(eff_id + 2) - eff_lower_(eff_id + 2);
+ m.pose.position.x = (eff_upper_(eff_id) + eff_lower_(eff_id)) / 2.0;
+ m.pose.position.y = (eff_upper_(eff_id + 1) + eff_lower_(eff_id + 1)) / 2.0;
+ m.pose.position.z = (eff_upper_(eff_id + 2) + eff_lower_(eff_id + 2)) / 2.0;
+ m.pose.orientation.w = 1.0;
+ m.color.r = 1.0;
+ m.color.a = 0.25;
+ ma.markers.emplace_back(m);
+ }
+ pub_markers_.publish(ma);
+}
+
+void EffBox::Instantiate(const EffBoxInitializer& init)
+{
+ parameters_ = init;
+ n_effs_ = frames_.size();
+ three_times_n_effs_ = 3 * n_effs_;
+
+ // Populate eff_upper_ and eff_lower_
+ eff_upper_.resize(3 * n_effs_, 1);
+ eff_lower_.resize(3 * n_effs_, 1);
+
+ for (int i = 0; i < n_effs_; ++i)
+ {
+ const int eff_id = 3 * i;
+
+ // Initialize frame and check user input
+ FrameWithBoxLimitsInitializer frame(parameters_.EndEffector[i]);
+ if (frame.XLim[0] > frame.XLim[1]) ThrowPretty("Specify XLim using lower then upper for end-effector " << i << ".");
+ if (frame.YLim[0] > frame.YLim[1]) ThrowPretty("Specify YLim using lower then upper for end-effector " << i << ".");
+ if (frame.ZLim[0] > frame.ZLim[1]) ThrowPretty("Specify ZLim using lower then upper for end-effector " << i << ".");
+
+ // Set upper and lower limits
+ eff_upper_[eff_id] = frame.XLim[1];
+ eff_upper_[eff_id + 1] = frame.YLim[1];
+ eff_upper_[eff_id + 2] = frame.ZLim[1];
+
+ eff_lower_[eff_id] = frame.XLim[0];
+ eff_lower_[eff_id + 1] = frame.YLim[0];
+ eff_lower_[eff_id + 2] = frame.ZLim[0];
+ }
+
+ if (debug_ && Server::IsRos())
+ {
+ pub_markers_ = Server::Advertise<visualization_msgs::MarkerArray>("eff_box_objects", 1, true);
+ visualization_msgs::Marker md; // delete previous markers
+ md.action = 3; // DELETEALL
+ visualization_msgs::MarkerArray ma;
+ ma.markers.reserve(1);
+ ma.markers.emplace_back(md);
+ pub_markers_.publish(ma);
+ }
+}
+
+int EffBox::TaskSpaceDim()
+{
+ return 6 * n_effs_;
+}
+} // namespace exotica

exotations/exotica_core_task_maps/src/task_map_py.cpp

@@ -35,6 +35,7 @@
 #include <exotica_core_task_maps/center_of_mass.h>
 #include <exotica_core_task_maps/distance.h>
 #include <exotica_core_task_maps/eff_axis_alignment.h>
+#include <exotica_core_task_maps/eff_box.h>
 #include <exotica_core_task_maps/eff_frame.h>
 #include <exotica_core_task_maps/eff_orientation.h>
 #include <exotica_core_task_maps/eff_position.h>
@@ -71,6 +72,10 @@ PYBIND11_MODULE(exotica_core_task_maps_py, module)
  .def("get_direction", &EffAxisAlignment::GetDirection)
  .def("set_direction", &EffAxisAlignment::SetDirection);
 
+ py::class_<EffBox, std::shared_ptr<EffBox>, TaskMap>(module, "EffBox")
+ .def("get_lower_limit", &EffBox::GetLowerLimit)
+ .def("get_upper_limit", &EffBox::GetUpperLimit);
+
  py::class_<PointToLine, std::shared_ptr<PointToLine>, TaskMap>(module, "PointToLine")
  .def_property("end_point", &PointToLine::GetEndPoint, &PointToLine::SetEndPoint);
 

exotica_core/cmake/generate_initializers.py

@@ -123,6 +123,8 @@ def parser(type):
  parser = "ParseVector<double,Eigen::Dynamic>"
  elif type == 'Eigen::Vector3d':
  parser = "ParseVector<double,3>"
+ elif type == 'Eigen::Vector2d':
+ parser = 'ParseVector<double,2>'
  elif type == 'bool':
  parser = "ParseBool"
  elif type == 'double':
@@ -387,7 +389,7 @@ def collect_extensions(input_files, search_dirs, content):
  for i in file_content['Include']:
  if not contains_include(i, content['Include']):
  content['Include'].append(i)
- 
+
  content['ClassName'] = class_name
  return content
 

exotica_examples/launch/python_ik_eff_box.launch

@@ -0,0 +1,13 @@
+<launch>
+
+ <arg name="debug" default="false" />
+ <arg unless="$(arg debug)" name="launch_prefix" value="" />
+ <arg if="$(arg debug)" name="launch_prefix" value="xterm -e gdb --args python" />
+
+ <param name="robot_description" textfile="$(find exotica_examples)/resources/robots/lwr_simplified.urdf" />
+ <param name="robot_description_semantic" textfile="$(find exotica_examples)/resources/robots/lwr_simplified.srdf" />
+
+ <node launch-prefix="$(arg launch_prefix)" pkg="exotica_examples" type="example_ik_eff_box" name="example_ik_node" output="screen" />
+
+ <node name="rviz" pkg="rviz" type="rviz" respawn="false" args="-d $(find exotica_examples)/resources/rviz_eff_box.rviz" />
+</launch>

exotica_examples/resources/configs/example_ik_eff_box.xml

@@ -0,0 +1,48 @@
+<?xml version="1.0" ?>
+<IKSolverDemoConfig>
+
+ <!--Example uses SciPy solver.-->
+
+ <EndPoseProblem Name="My Problem">
+
+ <PlanningScene>
+ <Scene>
+ <JointGroup>arm</JointGroup>
+ <URDF>{exotica_examples}/resources/robots/lwr_simplified.urdf</URDF>
+ <SRDF>{exotica_examples}/resources/robots/lwr_simplified.srdf</SRDF>
+ </Scene>
+ </PlanningScene>
+
+ <Maps>
+ <EffFrame Name="Position">
+ <EndEffector>
+ <Frame Link="lwr_arm_6_link" LinkOffset="0 0 0 0.7071067811865476 -4.3297802811774664e-17 0.7071067811865475 4.3297802811774664e-17"/>
+ </EndEffector>
+ </EffFrame>
+ <EffBox Name="EffBox" Debug="1">
+ <EndEffector>
+ <FrameWithBoxLimits Link="lwr_arm_6_link" XLim="0.5 0.75" YLim="-0.1 0.1" ZLim="0.25 0.65"/>
+ </EndEffector>
+ </EffBox>
+ </Maps>
+
+ <Cost>
+ <Task Task="Position"/>
+ </Cost>
+
+ <Equality>
+ </Equality>
+
+ <Inequality>
+ <Task Task="EffBox"/>
+ </Inequality>
+
+ <UseBounds>0</UseBounds>
+ <StartState>-0.1223444 -0.58650169 0.12824096 1.20356035 -0.07264356 -6.06820957 0.</StartState>
+
+ <NominalState>0 0 0 0 0 0 0</NominalState>
+ <W> 7 6 5 4 3 2 1 </W>
+
+ </EndPoseProblem>
+
+</IKSolverDemoConfig>