Commit ccd2efc4 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Fix copy construction of Device, HumanoidRobot and Gripper

parent d4f8ec3c
......@@ -388,14 +388,13 @@ namespace hpp {
/// \brief Initialization.
///
void init(const DeviceWkPtr_t& weakPtr);
//DEPREC /// \brief Initialization of of a clone device.
//DEPREC ///
//DEPREC void initCopy(const DeviceWkPtr_t& weakPtr, const Device& model);
/// \brief Initialization of of a clone device.
///
void initCopy(const DeviceWkPtr_t& weakPtr, const Device& other);
//DEPREC /// Recompute the number of distance pairs and resize the vector of distance results.
//DEPREC void updateDistances ();
private:
/// \brief Copy Constructor
Device(const Device& device);
......
......@@ -42,7 +42,7 @@ namespace hpp {
/// \param objectPositionInJoint object position in the the grasping
/// joint.
static GripperPtr_t create (const std::string& name,
const DevicePtr_t& device)
const DeviceWkPtr_t& device)
{
Gripper* ptr = new Gripper (name, device);
GripperPtr_t shPtr (ptr);
......@@ -50,6 +50,16 @@ namespace hpp {
return shPtr;
}
static GripperPtr_t createCopy (const GripperPtr_t& gripper,
const DeviceWkPtr_t& otherDevice)
{
Gripper* ptr = new Gripper (gripper->name(), otherDevice);
ptr->clearance(gripper->clearance());
GripperPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
/// Get joint to which the gripper is attached
const JointPtr_t& joint () const
{
......@@ -99,7 +109,7 @@ namespace hpp {
/// \param device
/// \todo device should be of type DeviceConstPtr_t but the constructor of
/// JointPtr_t needs a DevicePtr_t.
Gripper (const std::string& name, const DevicePtr_t& device);
Gripper (const std::string& name, const DeviceWkPtr_t& device);
void init (GripperWkPtr_t weakPtr)
{
......@@ -107,8 +117,10 @@ namespace hpp {
}
private:
inline DevicePtr_t device() const;
std::string name_;
DevicePtr_t device_;
DeviceWkPtr_t device_;
/// Joint of the robot that holds handles.
JointPtr_t joint_;
se3::FrameIndex fid_;
......
......@@ -102,11 +102,15 @@ namespace hpp {
/// \brief Constructor
HumanoidRobot (const std::string& name);
HumanoidRobot (const HumanoidRobot& other);
///
/// \brief Initialization.
///
void init (const HumanoidRobotWkPtr_t& weakPtr);
void initCopy (const HumanoidRobotWkPtr_t& weakPtr, const HumanoidRobot& other);
private:
HumanoidRobotWkPtr_t weakPtr_;
JointPtr_t waist_;
......
......@@ -31,6 +31,7 @@
#include <hpp/pinocchio/fwd.hh>
//#include <hpp/pinocchio/distance-result.hh>
#include <hpp/pinocchio/extra-config-space.hh>
#include <hpp/pinocchio/gripper.hh>
#include <hpp/pinocchio/joint.hh>
namespace hpp {
......@@ -54,6 +55,27 @@ namespace hpp {
createGeomData();
}
Device::Device(const Device& other)
: model_(other.model_)
, data_ (new Data (other.data()))
, geomModel_(other.geomModel_)
, geomData_ (new GeomData (other.geomData()))
, name_ (other.name_)
, jointVector_()
, currentConfiguration_ (other.currentConfiguration_)
, currentVelocity_ (other.currentVelocity_)
, currentAcceleration_ (other.currentAcceleration_)
, upToDate_ (false)
, geomUpToDate_ (false)
, computationFlag_ (other.computationFlag_)
, obstacles_()
, objectVector_ ()
, grippers_ ()
, extraConfigSpace_ (other.extraConfigSpace_)
, weakPtr_()
{
}
// static method
DevicePtr_t Device::
create (const std::string & name)
......@@ -65,11 +87,10 @@ namespace hpp {
// static method
DevicePtr_t Device::
createCopy (const DevicePtr_t& device)
createCopy (const DevicePtr_t& other)
{
DevicePtr_t res = Device::create(device->name()); // init shared ptr
res->model(device->modelPtr()); // Copy pointer to pinocchio model
res->createData(); // Create a new data, dont copy the pointer.
DevicePtr_t res = DevicePtr_t(new Device(*other)); // init shared ptr
res->initCopy (res, *other);
return res;
}
......@@ -93,6 +114,14 @@ namespace hpp {
objectVector_ = DeviceObjectVector(self);
}
void Device::initCopy(const DeviceWkPtr_t& weakPtr, const Device& other)
{
init(weakPtr);
grippers_.resize (other.grippers_.size());
for (std::size_t i = 0; i < grippers_.size(); ++i)
grippers_[i] = Gripper::createCopy(other.grippers_[i], weakPtr);
}
void Device::
createData()
{
......
......@@ -27,23 +27,26 @@
namespace hpp {
namespace pinocchio {
Gripper::Gripper (const std::string& name, const DevicePtr_t& device) :
Gripper::Gripper (const std::string& name, const DeviceWkPtr_t& device) :
name_ (name),
device_ (device),
clearance_ (0)
{
fid_ = device->model().getFrameId (name);
DevicePtr_t d = this->device();
fid_ = d->model().getFrameId (name);
// TODO as joint_ keeps a shared pointer to the device, the device will
// never be deleted.
joint_ = JointPtr_t (
new Joint(device, device->model().frames[fid_].parent));
new Joint(d, d->model().frames[fid_].parent));
}
const Transform3f& Gripper::objectPositionInJoint () const
{
return device_->model().frames[fid_].placement;
return device()->model().frames[fid_].placement;
}
GripperPtr_t Gripper::clone () const
{
GripperPtr_t self = weakPtr_.lock ();
return Gripper::create (name_, device_);
}
......@@ -61,5 +64,11 @@ namespace hpp {
return gripper.print (os);
}
DevicePtr_t Gripper::device() const
{
DevicePtr_t d = device_.lock();
assert (d);
return d;
}
} // namespace pinocchio
} // namespace hpp
......@@ -19,6 +19,8 @@
#include <hpp/pinocchio/humanoid-robot.hh>
#include <hpp/pinocchio/joint.hh>
namespace hpp {
namespace pinocchio {
......@@ -29,6 +31,15 @@ namespace hpp {
// ========================================================================
HumanoidRobot::HumanoidRobot (const HumanoidRobot& other)
: Device (other), weakPtr_ ()
, gazeOrigin_ (other.gazeOrigin_)
, gazeDirection_ (other.gazeDirection_)
{
}
// ========================================================================
HumanoidRobot::~HumanoidRobot ()
{
}
......@@ -52,6 +63,28 @@ namespace hpp {
weakPtr_ = weakPtr;
}
// ========================================================================
void HumanoidRobot::initCopy (const HumanoidRobotWkPtr_t& weakPtr, const HumanoidRobot& other)
{
// Cannot call HumanoidRobot::init because Device::init would be called
// twice.
weakPtr_ = weakPtr;
Device::initCopy (weakPtr, other);
// TODO the HumanoidRobot will be never be deleted as these joints have
// a shared pointer to the device.
DevicePtr_t d = weakPtr_.lock();
waist_ = JointPtr_t (new Joint (d, other.waist_ ->index()));
chest_ = JointPtr_t (new Joint (d, other.chest_ ->index()));
leftWrist_ = JointPtr_t (new Joint (d, other.leftWrist_ ->index()));
rightWrist_ = JointPtr_t (new Joint (d, other.rightWrist_->index()));
leftAnkle_ = JointPtr_t (new Joint (d, other.leftAnkle_ ->index()));
rightAnkle_ = JointPtr_t (new Joint (d, other.rightAnkle_->index()));
gazeJoint_ = JointPtr_t (new Joint (d, other.gazeJoint_ ->index()));
}
// ========================================================================
/// \brief Get Joint corresponding to the waist.
JointPtr_t HumanoidRobot::waist() const
{
......
Supports Markdown
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