From 1b6c76d941fc0929f3c83fc6b0ac1c7aaeb39799 Mon Sep 17 00:00:00 2001
From: Mathieu Geisert <mgeisert@laas.fr>
Date: Thu, 12 Jun 2014 16:25:29 +0200
Subject: [PATCH] change manipulation::gripper to model::gripper
---
include/hpp/manipulation/fwd.hh | 1 +
include/hpp/manipulation/robot.hh | 5 +++++
src/robot.cc | 22 +++++++++++++++++++++-
3 files changed, 27 insertions(+), 1 deletion(-)
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index f67f1f90..bf9bdf53 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -27,6 +27,7 @@ namespace hpp {
namespace manipulation {
typedef model::Device Device;
typedef model::DevicePtr_t DevicePtr_t;
+ typedef boost::shared_ptr <const Device> DeviceConstPtr_t;
typedef model::Joint Joint;
typedef model::JointPtr_t JointPtr_t;
typedef model::JointConstPtr_t JointConstPtr_t;
diff --git a/include/hpp/manipulation/robot.hh b/include/hpp/manipulation/robot.hh
index e3af6a8d..1214309c 100644
--- a/include/hpp/manipulation/robot.hh
+++ b/include/hpp/manipulation/robot.hh
@@ -136,6 +136,11 @@ namespace hpp {
const ObjectConstPtr_t& object);
/// Copy handles of the object into composite robot.
void copyHandles (const ObjectConstPtr_t& object);
+ /// Copy Device including kinematic chain.
+ void copyDevice (const JointPtr_t& rootJoint,
+ const DeviceConstPtr_t& device);
+ /// Copy grippers of the device into composite robot.
+ void copyGrippers (const DeviceConstPtr_t& device);
Devices_t robots_;
diff --git a/src/robot.cc b/src/robot.cc
index 15bff21b..6174a6ae 100644
--- a/src/robot.cc
+++ b/src/robot.cc
@@ -59,6 +59,25 @@ namespace hpp {
}
}
+ void Robot::copyDevice(const JointPtr_t& rootJoint,
+ const DeviceConstPtr_t& device)
+ {
+ copyKinematicChain (rootJoint, device->rootJoint ());
+ copyGrippers (device);
+ }
+
+ void Robot::copyGrippers (const DeviceConstPtr_t& device)
+ {
+ for (model::Grippers_t::const_iterator itGripper =
+ device->grippers ().begin ();
+ itGripper != device->grippers ().end (); ++itGripper) {
+ GripperPtr_t gripper = (*itGripper)->clone ();
+ gripper->name (device->name () + "/" + (*itGripper)->name ());
+ gripper->joint (jointMap_ [(*itGripper)->joint ()]);
+ addGripper (gripper->name (), gripper);
+ }
+ }
+
RobotPtr_t Robot::create (const std::string& name, const Devices_t& robots,
const Objects_t& objects)
{
@@ -110,7 +129,8 @@ namespace hpp {
// Copy robot kinematic chains
for (Devices_t::const_iterator itDev = robots_.begin ();
itDev != robots_.end (); ++itDev) {
- copyKinematicChain (rootJoint (), (*itDev)->rootJoint ());
+ //copyKinematicChain (rootJoint (), (*itDev)->rootJoint ());
+ copyDevice(rootJoint (), *itDev);
// Store rank of robot in configuration vector.
rankInConfiguration_ [(*itDev)] = rankInConfiguration;
rankInVelocity_ [(*itDev)] = rankInVelocity;
--
GitLab