Commit 8b735321 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

added clone methods for device

parent 02e3b06b
......@@ -89,7 +89,7 @@ namespace hpp {
positionInJointFrame_.setIdentity ();
}
/// Wrap fcl collision object and put at given position
explicit CollisionObject (fcl::CollisionGeometryPtr_t geometry,
explicit CollisionObject (const fcl::CollisionGeometryPtr_t geometry,
const Transform3f& position,
const std::string& name) :
object_ (new fcl::CollisionObject (geometry, position)),
......@@ -100,7 +100,8 @@ namespace hpp {
/// Copy constructor
explicit CollisionObject (const CollisionObject& object) :
object_ (object.object_),
object_ (new fcl::CollisionObject (object.fcl()->collisionGeometry(),
object.fcl()->getTransform())),
positionInJointFrame_ (object.positionInJointFrame_),
joint_ (0x0),
name_ (object.name_),
......
......@@ -333,9 +333,19 @@ namespace hpp {
///
void init(const DeviceWkPtr_t& weakPtr);
///
/// \brief Initialization of of a clone device.
///
void initCopy(const DeviceWkPtr_t& weakPtr, const Device& model);
/// Recompute number of distance pairs
void updateDistances ();
private:
/// \brief Copy Constructor
Device(const Device& device);
private:
void computeJointPositions ();
void computeJointJacobians ();
......
......@@ -44,10 +44,6 @@ namespace hpp {
com_.setZero ();
I4.setIdentity ();
}
// ========================================================================
Device::~Device()
{
}
......@@ -67,15 +63,77 @@ namespace hpp {
DevicePtr_t Device::createCopy(const DevicePtr_t& device)
{
Device* ptr = new Device(*device);
Device* ptr = new Device(device->name());
DevicePtr_t shPtr(ptr);
ptr->init (shPtr);
ptr->initCopy (shPtr, *device);
return shPtr;
}
// ========================================================================
namespace
{
void CloneJointRec(JointPtr_t current, JointPtr_t clone)
{
for(std::size_t i = 0; i < current->numberChildJoints(); ++i)
{
JointPtr_t cloneChild = current->childJoint(i)->clone();
clone->addChildJoint(cloneChild);
CloneJointRec(current->childJoint(i),cloneChild);
}
}
JointPtr_t CloneJoints(hpp::model::Device &clone, const hpp::model::Device &device)
{
JointPtr_t root(device.rootJoint()->clone());
clone.rootJoint(root);
CloneJointRec(device.rootJoint(), root);
return root;
}
JointVector_t CloneDisabledPositions(const hpp::model::Device &clone, const JointVector_t& disabledPositions)
{
JointVector_t res;
for(JointVector_t::const_iterator cit = disabledPositions.begin();
cit != disabledPositions.end(); ++cit)
{
res.push_back(clone.getJointByName((*cit)->name()));
}
return res;
}
Grippers_t CloneGrippers(hpp::model::Device &clone, const hpp::model::Device &device)
{
Grippers_t res;
for(Grippers_t::const_iterator cit = device.grippers().begin();
cit != device.grippers().end(); ++cit)
{
JointVector_t disabledPositions(
CloneDisabledPositions(clone, (*cit)->getDisabledCollisions()));
GripperPtr_t cloneGripper = Gripper::create((*cit)->name(),
clone.getJointByName((*cit)->joint()->name()),
(*cit)->objectPositionInJoint(),
disabledPositions);
res.push_back(cloneGripper);
}
return res;
}
void CloneCollisionPairs(hpp::model::Device &clone, const hpp::model::Device &device, Request_t type)
{
for(Device::CollisionPairs_t::const_iterator cit = device.collisionPairs(type).begin();
cit != device.collisionPairs(type).end(); ++cit)
{
clone.addCollisionPairs(clone.getJointByName(cit->first->name()),
clone.getJointByName(cit->second->name()),type);
}
}
}
// ========================================================================
DevicePtr_t Device::clone() const
{
return Device::createCopy(weakPtr_.lock());
......@@ -90,6 +148,21 @@ namespace hpp {
// ========================================================================
void Device::initCopy(const DeviceWkPtr_t& weakPtr, const Device& device)
{
init(weakPtr);
CloneJoints(*this, device);
grippers_ = CloneGrippers(*this, device);
setDimensionExtraConfigSpace(device.extraConfigSpace().dimension());
collisionPairs_.clear();
distances_.clear();
CloneCollisionPairs(*this, device,COLLISION);
CloneCollisionPairs(*this, device,DISTANCE);
computeDistances();
}
// ========================================================================
size_type Device::configSize () const
{
hppDout (info, configSize_ + extraConfigSpace_.dimension ());
......
......@@ -30,7 +30,7 @@ ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK -DBOOST_TEST_MAIN)
# Boost and add it to the test suite.
#
MACRO(HPP_MODEL_TEST NAME)
ADD_EXECUTABLE(${NAME} ${CMAKE_CURRENT_SOURCE_DIR}/${NAME}.cc)
ADD_EXECUTABLE(${NAME} ${CMAKE_CURRENT_SOURCE_DIR}/test-tools.hh ${CMAKE_CURRENT_SOURCE_DIR}/${NAME}.cc)
ADD_TEST(${NAME} ${RUNTIME_OUTPUT_DIRECTORY}/${NAME})
PKG_CONFIG_USE_DEPENDENCY(${NAME} eigen3)
......@@ -43,4 +43,9 @@ MACRO(HPP_MODEL_TEST NAME)
${PROJECT_NAME})
ENDMACRO(HPP_MODEL_TEST)
SET(${PROJECT_NAME}_HEADERS
tests/test-tools.hh
)
HPP_MODEL_TEST (test-configuration)
HPP_MODEL_TEST (test-clone)
///
/// Copyright (c) 2016 CNRS
/// Author: Steve Tonneau
///
///
// This file is part of hpp-model
// hpp-model is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-model is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-model If not, see
// <http://www.gnu.org/licenses/>.
// This test
// - Builds and clones collision-object,
// - asserts that they can displaced treated independtly
// - while the core geometry object remains the same
// - Similarly, it builds a robot
// - and clones it, and tests that the robot
// - are equivalent and independent
#include "test-tools.hh"
#include <hpp/fcl/collision.h>
BOOST_AUTO_TEST_CASE(clone_collision_object)
{
JointPtr_t joint(0);
CollisionObjectPtr_t c1 = createCollisionObject();
CollisionObjectPtr_t c2 = c1->clone(joint);
BOOST_CHECK_EQUAL(c1->name(), c2->name());
BOOST_CHECK_EQUAL(c1->fcl()->collisionGeometry().get(), c2->fcl()->collisionGeometry().get());
BOOST_CHECK_NE(c1->fcl().get(), c2->fcl().get());
fcl::CollisionResult collisionResult;
fcl::CollisionRequest collisionRequest;
fcl::collide(c1->fcl().get(),c2->fcl().get(),collisionRequest,collisionResult);
BOOST_CHECK_MESSAGE ( collisionResult.isCollision(),
"Object and clone should be colliding in default position");
fcl::CollisionResult colRes2;
fcl::CollisionRequest colReq2;
fcl::Transform3f transform;
transform.setTranslation(fcl::Vec3f(20,2,2));
c1->move(transform);
fcl::collide(c1->fcl().get(),c2->fcl().get(),colReq2,colRes2);
BOOST_CHECK_MESSAGE ( !colRes2.isCollision(),
"Object and clone should not be colliding after translation");
}
BOOST_AUTO_TEST_CASE(clone_device)
{
DevicePtr_t d1 = createRobotWithExtraDof();
DevicePtr_t d2 = d1->clone();
BOOST_CHECK_EQUAL(d1->name(), d2->name());
BOOST_CHECK_EQUAL(d1->extraConfigSpace().dimension(), d2->extraConfigSpace().dimension());
BOOST_CHECK_EQUAL(d1->configSize(), d2->configSize());
JointVector_t jv1 = d1->getJointVector();
JointVector_t jv2 = d2->getJointVector();
BOOST_CHECK_EQUAL(jv1.size(),jv2.size());
JointPtr_t jtree1(d1->rootJoint()), jtree2(d2->rootJoint());
while(jtree1 && jtree2)
{
BOOST_CHECK_EQUAL(jtree1->name(), jtree2->name());
BOOST_CHECK_EQUAL(jtree1->numberChildJoints(), jtree2->numberChildJoints());
jtree1 = (jtree1->numberChildJoints() > 0) ? (jtree1->childJoint(0)) : 0;
jtree2 = (jtree2->numberChildJoints() > 0) ? (jtree2->childJoint(0)) : 0;
}
d1->computeForwardKinematics();
d2->computeForwardKinematics();
//collision checking with objects
fcl::CollisionObjectPtr_t c1 = d1->rootJoint()->linkedBody()->innerObjects(hpp::model::COLLISION).front()->fcl();
fcl::CollisionObjectPtr_t c2 = d2->rootJoint()->linkedBody()->innerObjects(hpp::model::COLLISION).front()->fcl();
fcl::CollisionResult collisionResult;
fcl::CollisionRequest collisionRequest;
fcl::collide(c1.get(),c2.get(),collisionRequest,collisionResult);
BOOST_CHECK_MESSAGE ( collisionResult.isCollision(),
"Object and clone should be colliding in default position");
Configuration_t config = d2->currentConfiguration();
config.head(2)= Eigen::Vector2d(5,5);
d2->currentConfiguration(config); d2->computeForwardKinematics();
fcl::CollisionResult colRes2;
fcl::CollisionRequest colReq2;
fcl::collide(c1.get(),c2.get(),colReq2,colRes2);
BOOST_CHECK_MESSAGE ( !colRes2.isCollision(),
"Device robot and clone should not be colliding after collision");
}
......@@ -23,138 +23,7 @@
// - test consistency between hpp::model::difference and
// hpp::model::integrate functions.
#include <sstream>
#define BOOST_TEST_MODULE TEST_CONFIGURATION
#include <boost/test/unit_test.hpp>
#include <boost/test/output_test_stream.hpp>
using boost::test_tools::output_test_stream;
#include <Eigen/Geometry>
#include <hpp/util/debug.hh>
#include <hpp/model/configuration.hh>
#include <hpp/model/object-factory.hh>
#include <hpp/model/fcl-to-eigen.hh>
using hpp::model::vector_t;
using hpp::model::vectorIn_t;
using hpp::model::Configuration_t;
using hpp::model::JointPtr_t;
using hpp::model::ObjectFactory;
using hpp::model::Transform3f;
using fcl::Quaternion3f;
using hpp::model::Device;
using hpp::model::DevicePtr_t;
using hpp::model::JointVector_t;
using hpp::model::ConfigurationPtr_t;
using hpp::model::size_type;
using hpp::model::value_type;
typedef Eigen::AngleAxis <value_type> AngleAxis_t;
typedef Eigen::Quaternion <value_type> Quaternion_t;
// Create a robot with various types of joints
DevicePtr_t createRobot ()
{
DevicePtr_t robot = Device::create ("robot");
Transform3f position; position.setIdentity ();
ObjectFactory factory;
// planar root joint
JointPtr_t root = factory.createJointTranslation2 (position);
robot->rootJoint (root);
root->isBounded (0, true);
root->lowerBound (0, -1);
root->upperBound (0, 1);
root->isBounded (1, true);
root->lowerBound (1, -1);
root->upperBound (1, 1);
// Rotation around z
position.setQuatRotation (Quaternion3f (sqrt (2)/2, 0, -sqrt (2)/2, 0));
JointPtr_t joint = factory.createUnBoundedJointRotation (position);
root->addChildJoint (joint);
position.setIdentity ();
// SO3 joint
position.setIdentity ();
JointPtr_t j1 = factory.createJointSO3 (position);
j1->name ("so3");
joint->addChildJoint (j1);
return robot;
}
void shootRandomConfig (const DevicePtr_t& robot, Configuration_t& config)
{
JointVector_t jv = robot->getJointVector ();
for (JointVector_t::const_iterator itJoint = jv.begin ();
itJoint != jv.end (); itJoint++) {
std::size_t rank = (*itJoint)->rankInConfiguration ();
(*itJoint)->configuration ()->uniformlySample (rank, config);
}
// Shoot extra configuration variables
size_type extraDim = robot->extraConfigSpace ().dimension ();
size_type offset = robot->configSize () - extraDim;
for (size_type i=0; i<extraDim; ++i) {
value_type lower = robot->extraConfigSpace ().lower (i);
value_type upper = robot->extraConfigSpace ().upper (i);
value_type range = upper - lower;
if ((range < 0) ||
(range == std::numeric_limits<double>::infinity())) {
std::ostringstream oss
("Cannot uniformy sample extra config variable ");
oss << i << ". min = " << ", max = " << upper << std::endl;
throw std::runtime_error (oss.str ());
}
config [offset + i] = (upper - lower) * rand ()/RAND_MAX;
}
}
vector_t slerp (vectorIn_t v0, vectorIn_t v1, const value_type t) {
Quaternion_t q0(v0[0], v0[1], v0[2], v0[3]);
Quaternion_t q1(v1[0], v1[1], v1[2], v1[3]);
Quaternion_t q = q0.slerp (t, q1);
vector_t res(4);
res[0] = q.w(); res[1] = q.x(); res[2] = q.y(); res[3] = q.z();
return res;
}
AngleAxis_t aa (vectorIn_t v0, vectorIn_t v1) {
Quaternion_t q0(v0[0], v0[1], v0[2], v0[3]);
Quaternion_t q1(v1[0], v1[1], v1[2], v1[3]);
return AngleAxis_t (q0 * q1.conjugate ());
}
vector_t interpolation (DevicePtr_t robot, vectorIn_t q0, vectorIn_t q1, int n)
{
const size_type rso3 = 4;
bool print = false;
vector_t q2 (q0);
value_type u = 0;
value_type step = (value_type)1 / (value_type)(n + 2);
vector_t angle1 (n+2), angle2(n+2);
if (print) std::cout << "HPP : ";
for (int i = 0; i < n + 2; ++i) {
u = 0 + i * step;
hpp::model::interpolate (robot, q0, q1, u, q2);
AngleAxis_t aa1 (aa (q0.segment<4>(rso3),q2.segment<4>(rso3)));
angle1[i] = aa1.angle ();
if (print) std::cout << aa1.angle () << ", ";
}
if (print) std::cout << "\n";
if (print) std::cout << "Eigen: ";
for (int i = 0; i < n + 2; ++i) {
u = 0 + i * step;
vector_t eigen_slerp = slerp (q0.segment<4>(rso3), q1.segment<4>(rso3), u);
AngleAxis_t aa2 (aa (q0.segment<4>(rso3),eigen_slerp));
angle2[i] = aa2.angle ();
if (print) std::cout << aa2.angle () << ", ";
}
if (print) std::cout << "\n";
return angle1 - angle2;
}
#include "test-tools.hh"
BOOST_AUTO_TEST_CASE(difference_and_integrate)
{
......
// Copyright (C) 2014 LAAS-CNRS
// Author: Steve Tonneau
//
// This file is part of the hpp-rbprm.
//
// hpp-core is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// test-hpp is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with hpp-core. If not, see <http://www.gnu.org/licenses/>.
#include <sstream>
#define BOOST_TEST_MODULE TEST_CONFIGURATION
#include <boost/test/unit_test.hpp>
#include <boost/test/output_test_stream.hpp>
using boost::test_tools::output_test_stream;
#include <Eigen/Geometry>
#include <hpp/util/debug.hh>
#include <hpp/model/configuration.hh>
#include <hpp/model/collision-object.hh>
#include <hpp/model/object-factory.hh>
#include <hpp/model/fcl-to-eigen.hh>
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/shape/geometric_shapes.h>
using hpp::model::Body;
using hpp::model::BodyPtr_t;
using hpp::model::vector_t;
using hpp::model::vectorIn_t;
using hpp::model::Configuration_t;
using hpp::model::JointPtr_t;
using hpp::model::ObjectFactory;
using hpp::model::Transform3f;
using fcl::Quaternion3f;
using hpp::model::Device;
using hpp::model::DevicePtr_t;
using hpp::model::JointVector_t;
using hpp::model::ConfigurationPtr_t;
using hpp::model::size_type;
using hpp::model::value_type;
using hpp::model::CollisionObject;
using hpp::model::CollisionObjectPtr_t;
using fcl::CollisionGeometry;
using fcl::CollisionGeometryPtr_t;
typedef Eigen::AngleAxis <value_type> AngleAxis_t;
typedef Eigen::Quaternion <value_type> Quaternion_t;
namespace
{
CollisionObjectPtr_t createCollisionObject(const std::string& name = "obj1")
{
CollisionGeometryPtr_t root (new fcl::Box (1, 1, 1));
CollisionObjectPtr_t obstacle = CollisionObject::create
(root, fcl::Transform3f (), name);
return obstacle;
}
// Create a robot with various types of joints
DevicePtr_t createRobot ()
{
DevicePtr_t robot = Device::create ("robot");
Transform3f position; position.setIdentity ();
ObjectFactory factory;
// planar root joint
JointPtr_t root = factory.createJointTranslation2 (position);
root->name("rootJoint");
robot->rootJoint (root);
root->isBounded (0, true);
root->lowerBound (0, -1);
root->upperBound (0, 1);
root->isBounded (1, true);
root->lowerBound (1, -1);
root->upperBound (1, 1);
BodyPtr_t body = new Body;
body->name ("root");
root->setLinkedBody (body);
body->addInnerObject(createCollisionObject(), true, true);
// Rotation around z
position.setQuatRotation (Quaternion3f (sqrt (2)/2, 0, -sqrt (2)/2, 0));
JointPtr_t joint = factory.createUnBoundedJointRotation (position);
joint->name("j0");
root->addChildJoint (joint);
position.setIdentity ();
// SO3 joint
position.setIdentity ();
JointPtr_t j1 = factory.createJointSO3 (position);
j1->name ("so3");
joint->addChildJoint (j1);
return robot;
}
DevicePtr_t createRobotWithExtraDof ()
{
DevicePtr_t robot =createRobot();
robot->setDimensionExtraConfigSpace(3);
return robot;
}
void shootRandomConfig (const DevicePtr_t& robot, Configuration_t& config)
{
JointVector_t jv = robot->getJointVector ();
for (JointVector_t::const_iterator itJoint = jv.begin ();
itJoint != jv.end (); itJoint++) {
std::size_t rank = (*itJoint)->rankInConfiguration ();
(*itJoint)->configuration ()->uniformlySample (rank, config);
}
// Shoot extra configuration variables
size_type extraDim = robot->extraConfigSpace ().dimension ();
size_type offset = robot->configSize () - extraDim;
for (size_type i=0; i<extraDim; ++i) {
value_type lower = robot->extraConfigSpace ().lower (i);
value_type upper = robot->extraConfigSpace ().upper (i);
value_type range = upper - lower;
if ((range < 0) ||
(range == std::numeric_limits<double>::infinity())) {
std::ostringstream oss
("Cannot uniformy sample extra config variable ");
oss << i << ". min = " << ", max = " << upper << std::endl;
throw std::runtime_error (oss.str ());
}
config [offset + i] = (upper - lower) * rand ()/RAND_MAX;
}
}
vector_t slerp (vectorIn_t v0, vectorIn_t v1, const value_type t) {
Quaternion_t q0(v0[0], v0[1], v0[2], v0[3]);
Quaternion_t q1(v1[0], v1[1], v1[2], v1[3]);
Quaternion_t q = q0.slerp (t, q1);
vector_t res(4);
res[0] = q.w(); res[1] = q.x(); res[2] = q.y(); res[3] = q.z();
return res;
}
AngleAxis_t aa (vectorIn_t v0, vectorIn_t v1) {
Quaternion_t q0(v0[0], v0[1], v0[2], v0[3]);
Quaternion_t q1(v1[0], v1[1], v1[2], v1[3]);
return AngleAxis_t (q0 * q1.conjugate ());
}
vector_t interpolation (DevicePtr_t robot, vectorIn_t q0, vectorIn_t q1, int n)
{
const size_type rso3 = 4;
bool print = false;
vector_t q2 (q0);
value_type u = 0;
value_type step = (value_type)1 / (value_type)(n + 2);
vector_t angle1 (n+2), angle2(n+2);
if (print) std::cout << "HPP : ";
for (int i = 0; i < n + 2; ++i) {
u = 0 + i * step;
hpp::model::interpolate (robot, q0, q1, u, q2);
AngleAxis_t aa1 (aa (q0.segment<4>(rso3),q2.segment<4>(rso3)));
angle1[i] = aa1.angle ();
if (print) std::cout << aa1.angle () << ", ";
}
if (print) std::cout << "\n";
if (print) std::cout << "Eigen: ";
for (int i = 0; i < n + 2; ++i) {
u = 0 + i * step;
vector_t eigen_slerp = slerp (q0.segment<4>(rso3), q1.segment<4>(rso3), u);
AngleAxis_t aa2 (aa (q0.segment<4>(rso3),eigen_slerp));
angle2[i] = aa2.angle ();
if (print) std::cout << aa2.angle () << ", ";
}
if (print) std::cout << "\n";
return angle1 - angle2;
}
}
Markdown is supported
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