Skip to content
Snippets Groups Projects
Commit f109a665 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Update Handle::Create(Pre)Grasp to new definition of transformation constraint.

parent 891ca966
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/axial-handle.cc
+ 38
− 33
View file @ f109a665
...@@ -35,61 +35,66 @@ namespace hpp { ...@@ -35,61 +35,66 @@ namespace hpp {
(const GripperPtr_t& gripper) const (const GripperPtr_t& gripper) const
{ {
using boost::assign::list_of; using boost::assign::list_of;
std::vector <bool> mask = list_of (true)(true)(true)(false)(true)(true); std::vector <bool> mask = list_of (true)(true)(true)(true)(true)(false);
Transform3f transform = inverse (localPosition()) * gripper->objectPositionInJoint (); return RelativeTransformation::create
return RelativeTransformation::create ("Transformation_(1,1,1,0,1,1)_" + name () + "_" + gripper->name (), ("Transformation_(1,1,1,1,1,1)_" + name () + "_" + gripper->name (),
gripper->joint()->robot(), gripper->joint (), joint(), gripper->joint()->robot(), gripper->joint (), joint (),
inverse (localPosition()) * gripper->objectPositionInJoint (), mask); gripper->objectPositionInJoint (), localPosition(), mask);
} }
DifferentiableFunctionPtr_t AxialHandle::createGraspComplement DifferentiableFunctionPtr_t AxialHandle::createGraspComplement
(const GripperPtr_t& gripper) const (const GripperPtr_t& gripper) const
{ {
using boost::assign::list_of; using boost::assign::list_of;
std::vector <bool> mask = list_of (true)(false)(false); std::vector <bool> mask = list_of (false)(false)(false)(false)(true)
Transform3f transform = inverse (localPosition()) * gripper->objectPositionInJoint (); (false);
return RelativeOrientation::create ("Orientation_(1,0,0)_" + name () + "_" + gripper->name (), return RelativeTransformation::create
gripper->joint()->robot(), gripper->joint (), joint(), transform.getRotation (), mask); ("Transformation_(0,0,0,0,0,0)_" + name () + "_" + gripper->name (),
gripper->joint()->robot(), gripper->joint (), joint (),
gripper->objectPositionInJoint (), localPosition(), mask);
} }
DifferentiableFunctionPtr_t AxialHandle::createPreGrasp DifferentiableFunctionPtr_t AxialHandle::createPreGrasp
(const GripperPtr_t& gripper) const (const GripperPtr_t& gripper) const
{ {
using boost::assign::list_of; using boost::assign::list_of;
std::vector <bool> mask = list_of (false)(true)(true)(false)(true)(true); std::vector <bool> mask = list_of (false)(true)(true)(true)(true)(false);
return RelativeTransformation::create ("Transformation_(0,1,1,0,1,1)_" + name () + "_" + gripper->name (), return RelativeTransformation::create
gripper->joint()->robot(), gripper->joint (), joint(), ("Transformation_(0,1,1,1,1,1)_" + name () + "_" + gripper->name (),
inverse (localPosition()) * gripper->objectPositionInJoint (), mask); gripper->joint()->robot(), gripper->joint (), joint (),
gripper->objectPositionInJoint (), localPosition(), mask);
} }
DifferentiableFunctionPtr_t AxialHandle::createPreGraspComplement DifferentiableFunctionPtr_t AxialHandle::createPreGraspComplement
(const GripperPtr_t& gripper) const (const GripperPtr_t& gripper) const
{ {
using boost::assign::list_of; using boost::assign::list_of;
std::vector <bool> mask = list_of (true)(false)(false); std::vector <bool> mask = list_of (true)(false)(false)(false)(true)
Transform3f transform = inverse (localPosition()) * gripper->objectPositionInJoint (); (false);
fcl::Vec3f target = transform.getTranslation () Transform3f transform (gripper->objectPositionInJoint ().getRotation (),
+ fcl::Vec3f (clearance () + gripper->clearance (),0,0); gripper->objectPositionInJoint ().getTranslation ()
return RelativePosition::create ("Position_(1,0,0)_" + name () + "_" + gripper->name (), + fcl::Vec3f (gripper->clearance () +
gripper->joint()->robot(), gripper->joint (), joint(), target, fcl::Vec3f (0,0,0), mask); clearance (),0,0));
return RelativeTransformation::create
("Transformation_(1,0,0,0,0,0)_" + name () + "_" + gripper->name (),
gripper->joint()->robot(), gripper->joint (), joint (),
transform, localPosition(), mask);
} }
DifferentiableFunctionPtr_t AxialHandle::createPreGraspComplement DifferentiableFunctionPtr_t AxialHandle::createPreGraspComplement
(const GripperPtr_t& gripper, const value_type& shift) const (const GripperPtr_t& gripper, const value_type& shift) const
{ {
//using boost::assign::list_of;
//std::vector <bool> mask = list_of (true)(false)(false)(true)(false)(false);
//Transform3f transform = inverse (localPosition()) * gripper->objectPositionInJoint ();
//transform.setTranslation (transform.getTranslation () + fcl::Vec3f (shift,0,0));
//return RelativeTransformation::create
//(gripper->joint()->robot(), gripper->joint (), joint(), transform, mask);
using boost::assign::list_of; using boost::assign::list_of;
std::vector <bool> mask = list_of (true)(false)(false); std::vector <bool> mask = list_of (true)(false)(false)(false)(true)
Transform3f transform = inverse (localPosition()) * gripper->objectPositionInJoint (); (false);
fcl::Vec3f target = transform.getTranslation () + fcl::Vec3f (shift,0,0); Transform3f transform (gripper->objectPositionInJoint ().getRotation (),
return RelativePosition::create ("Position_(1,0,0)_" + name () + "_" + gripper->name (), gripper->objectPositionInJoint ().getTranslation ()
gripper->joint()->robot(), gripper->joint (), joint(), target, fcl::Vec3f (0,0,0), mask); + fcl::Vec3f (gripper->clearance () +
clearance () + shift,0,0));
return RelativeTransformation::create
("Transformation_(1,0,0,0,0,0)_" + name () + "_" + gripper->name (),
gripper->joint()->robot(), gripper->joint (), joint (),
transform, localPosition(), mask);
} }
HandlePtr_t AxialHandle::clone () const HandlePtr_t AxialHandle::clone () const
......
src/handle.cc
+ 35
− 21
View file @ f109a665
...@@ -17,6 +17,7 @@ ...@@ -17,6 +17,7 @@
// hpp-manipulation. If not, see // hpp-manipulation. If not, see
// <http://www.gnu.org/licenses/>. // <http://www.gnu.org/licenses/>.
#include <hpp/util/debug.hh>
#include <hpp/manipulation/handle.hh> #include <hpp/manipulation/handle.hh>
#include <boost/assign/list_of.hpp> #include <boost/assign/list_of.hpp>
...@@ -37,19 +38,22 @@ namespace hpp { ...@@ -37,19 +38,22 @@ namespace hpp {
{ {
using boost::assign::list_of; using boost::assign::list_of;
std::vector <bool> mask = list_of (true)(true)(true)(true)(true)(true); std::vector <bool> mask = list_of (true)(true)(true)(true)(true)(true);
return RelativeTransformation::create ("Transformation_(1,1,1,1,1,1)_" + name () + "_" + gripper->name (), return RelativeTransformation::create
gripper->joint()->robot(), gripper->joint (), joint(), ("Transformation_(1,1,1,1,1,1)_" + name () + "_" + gripper->name (),
inverse (localPosition()) * gripper->objectPositionInJoint (), mask); gripper->joint()->robot(), gripper->joint (), joint (),
gripper->objectPositionInJoint (), localPosition(), mask);
} }
DifferentiableFunctionPtr_t Handle::createGraspComplement DifferentiableFunctionPtr_t Handle::createGraspComplement
(const GripperPtr_t& gripper) const (const GripperPtr_t& gripper) const
{ {
using boost::assign::list_of; using boost::assign::list_of;
std::vector <bool> mask = list_of (false)(false)(false)(false)(false)(false); std::vector <bool> mask = list_of (false)(false)(false)(false)(false)
return RelativeTransformation::create ("Transformation_(0,0,0,0,0,0)_" + name () + "_" + gripper->name (), (false);
gripper->joint()->robot(), gripper->joint (), joint(), return RelativeTransformation::create
inverse (localPosition()) * gripper->objectPositionInJoint (), mask); ("Transformation_(0,0,0,0,0,0)_" + name () + "_" + gripper->name (),
gripper->joint()->robot(), gripper->joint (), joint (),
gripper->objectPositionInJoint (), localPosition(), mask);
} }
DifferentiableFunctionPtr_t Handle::createPreGrasp DifferentiableFunctionPtr_t Handle::createPreGrasp
...@@ -57,32 +61,42 @@ namespace hpp { ...@@ -57,32 +61,42 @@ namespace hpp {
{ {
using boost::assign::list_of; using boost::assign::list_of;
std::vector <bool> mask = list_of (false)(true)(true)(true)(true)(true); std::vector <bool> mask = list_of (false)(true)(true)(true)(true)(true);
Transform3f transform = inverse (localPosition()) * gripper->objectPositionInJoint (); return RelativeTransformation::create
return RelativeTransformation::create ("Transformation_(0,1,1,1,1,1)_" + name () + "_" + gripper->name (), ("Transformation_(0,1,1,1,1,1)_" + name () + "_" + gripper->name (),
gripper->joint()->robot(), gripper->joint (), joint(), transform, mask); gripper->joint()->robot(), gripper->joint (), joint (),
gripper->objectPositionInJoint (), localPosition(), mask);
} }
DifferentiableFunctionPtr_t Handle::createPreGraspComplement DifferentiableFunctionPtr_t Handle::createPreGraspComplement
(const GripperPtr_t& gripper) const (const GripperPtr_t& gripper) const
{ {
using boost::assign::list_of; using boost::assign::list_of;
std::vector <bool> mask = list_of (true)(false)(false); std::vector <bool> mask = list_of (true)(false)(false)(false)(false)
Transform3f transform = inverse (localPosition()) * gripper->objectPositionInJoint (); (false);
fcl::Vec3f target = transform.getTranslation () Transform3f transform (gripper->objectPositionInJoint ().getRotation (),
+ fcl::Vec3f (clearance () + gripper->clearance (),0,0); gripper->objectPositionInJoint ().getTranslation ()
return RelativePosition::create ("Position_(1,0,0)_" + name () + "_" + gripper->name (), + fcl::Vec3f (gripper->clearance () +
gripper->joint()->robot(), gripper->joint (), joint(), target, fcl::Vec3f (0,0,0), mask); clearance (),0,0));
return RelativeTransformation::create
("Transformation_(1,0,0,0,0,0)_" + name () + "_" + gripper->name (),
gripper->joint()->robot(), gripper->joint (), joint (),
transform, localPosition(), mask);
} }
DifferentiableFunctionPtr_t Handle::createPreGraspComplement DifferentiableFunctionPtr_t Handle::createPreGraspComplement
(const GripperPtr_t& gripper, const value_type& shift) const (const GripperPtr_t& gripper, const value_type& shift) const
{ {
using boost::assign::list_of; using boost::assign::list_of;
std::vector <bool> mask = list_of (true)(false)(false); std::vector <bool> mask = list_of (true)(false)(false)(false)(false)
Transform3f transform = inverse (localPosition()) * gripper->objectPositionInJoint (); (false);
fcl::Vec3f target = transform.getTranslation () + fcl::Vec3f (shift,0,0); Transform3f transform (gripper->objectPositionInJoint ().getRotation (),
return RelativePosition::create ("Position_(1,0,0)_" + name () + "_" + gripper->name (), gripper->objectPositionInJoint ().getTranslation ()
gripper->joint()->robot(), gripper->joint (), joint(), target, fcl::Vec3f (0,0,0), mask); + fcl::Vec3f (gripper->clearance () +
clearance () + shift,0,0));
return RelativeTransformation::create
("Transformation_(1,0,0,0,0,0)_" + name () + "_" + gripper->name (),
gripper->joint()->robot(), gripper->joint (), joint (),
transform, localPosition(), mask);
} }
HandlePtr_t Handle::clone () const HandlePtr_t Handle::clone () const
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment