diff --git a/nextage_description/models/main.dae b/nextage_description/models/main.dae
index edddfb88..970f03d1 100644
--- a/nextage_description/models/main.dae
+++ b/nextage_description/models/main.dae
@@ -6444,6 +6444,24 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+
+
+
+ -0.035 0 0
+ 0 1 0 -90.00000001175157
+
+
+
+
+
+
+
+ -0.035 0 0
+ 0 1 0 -90.00000001175157
+
+
+
@@ -8725,6 +8743,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
640 480 3
0.033333
+
+ -1.000000 -1.000000 -1.000000
+ -1.000000 -1.000000 -1.000000
+
+
+ -1.000000 -1.000000 -1.000000
+ -1.000000 -1.000000 -1.000000
+
diff --git a/nextage_description/models/main.wrl b/nextage_description/models/main.wrl
index 06decef1..e176bc68 100644
--- a/nextage_description/models/main.wrl
+++ b/nextage_description/models/main.wrl
@@ -577,6 +577,11 @@ DEF HiroNX Humanoid{
mass 1.05156
momentsOfInertia [ 0.00194072 -1.1E-7 -4.2482E-4 -1.1E-7 0.00209392 -1.2E-7 -4.2482E-4 -1.2E-7 3.5788E-4 ]
children [
+ DEF rhsensor ForceSensor {
+ translation -0.035 0 0
+ rotation 0 1 0 -1.570796327 # Toyota Lab setting
+ sensorId 0 # need to start from 0 for each sensor type
+ }
Transform {
translation -0.0061 0.0 -0.002
children Shape {
@@ -765,6 +770,11 @@ DEF HiroNX Humanoid{
mass 1.05156
momentsOfInertia [ 0.00194072 1.1E-7 -4.2482E-4 1.1E-7 0.00209392 1.2E-7 -4.2482E-4 1.2E-7 3.5788E-4 ]
children[
+ DEF lhsensor ForceSensor {
+ translation -0.035 0 0
+ rotation 0 1 0 -1.570796327
+ sensorId 1
+ }
Transform {
#translation -0.0225 0.010 -0.0301
translation -0.0061 0.0 -0.002
diff --git a/nextage_ros_bridge/conf/.gitignore b/nextage_ros_bridge/conf/.gitignore
new file mode 100644
index 00000000..c8eb96fd
--- /dev/null
+++ b/nextage_ros_bridge/conf/.gitignore
@@ -0,0 +1,8 @@
+*~
+nextage.RobotHardware.conf
+nextage.conf
+nextage.xml
+nextage_nosim.RobotHardware.conf
+nextage_nosim.conf
+nextage_nosim.xml
+
diff --git a/nextage_ros_bridge/conf/nextage.RobotHardware.conf.in b/nextage_ros_bridge/conf/nextage.RobotHardware.conf.in
deleted file mode 100644
index a59ccc1a..00000000
--- a/nextage_ros_bridge/conf/nextage.RobotHardware.conf.in
+++ /dev/null
@@ -1,4 +0,0 @@
-model: file://@PROJECT_SOURCE_DIR@/models/@ROBOT_NAME@.dae
-exec_cxt.periodic.type: hrpExecutionContext
-exec_cxt.periodic.rate: 200
-
diff --git a/nextage_ros_bridge/conf/nextage.conf.in b/nextage_ros_bridge/conf/nextage.conf.in
deleted file mode 100644
index 43634957..00000000
--- a/nextage_ros_bridge/conf/nextage.conf.in
+++ /dev/null
@@ -1,4 +0,0 @@
-collision_pair: RARM_JOINT2:LARM_JOINT2 RARM_JOINT3:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5
-model: model: file://@PROJECT_SOURCE_DIR@/models/@ROBOT_NAME@.dae
-dt: 0.005
-
diff --git a/nextage_ros_bridge/conf/nextage.xml.in b/nextage_ros_bridge/conf/nextage.xml.in
deleted file mode 100644
index 6fed8886..00000000
--- a/nextage_ros_bridge/conf/nextage.xml.in
+++ /dev/null
@@ -1,172 +0,0 @@
-
-
- -
-
-
-
-
-
- -
-
-
-
-
-
-
-
- -
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- -
-
-
-
-
-
-
-
- -
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- -
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/nextage_ros_bridge/conf/nextage_nosim.RobotHardware.conf.in b/nextage_ros_bridge/conf/nextage_nosim.RobotHardware.conf.in
deleted file mode 100644
index a59ccc1a..00000000
--- a/nextage_ros_bridge/conf/nextage_nosim.RobotHardware.conf.in
+++ /dev/null
@@ -1,4 +0,0 @@
-model: file://@PROJECT_SOURCE_DIR@/models/@ROBOT_NAME@.dae
-exec_cxt.periodic.type: hrpExecutionContext
-exec_cxt.periodic.rate: 200
-
diff --git a/nextage_ros_bridge/conf/nextage_nosim.conf.in b/nextage_ros_bridge/conf/nextage_nosim.conf.in
deleted file mode 100644
index d6d2f4e9..00000000
--- a/nextage_ros_bridge/conf/nextage_nosim.conf.in
+++ /dev/null
@@ -1,3 +0,0 @@
-model: file://@PROJECT_SOURCE_DIR@/models/@ROBOT_NAME@.dae
-dt: 0.005
-
diff --git a/nextage_ros_bridge/conf/nextage_nosim.xml.in b/nextage_ros_bridge/conf/nextage_nosim.xml.in
deleted file mode 100644
index 756ebec2..00000000
--- a/nextage_ros_bridge/conf/nextage_nosim.xml.in
+++ /dev/null
@@ -1,172 +0,0 @@
-
-
- -
-
-
-
-
-
- -
-
-
-
-
-
-
-
- -
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- -
-
-
-
-
-
-
-
- -
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- -
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/nextage_ros_bridge/launch/nextage_ros_bridge_real.launch b/nextage_ros_bridge/launch/nextage_ros_bridge_real.launch
index 7ff06a57..bfd5ec10 100644
--- a/nextage_ros_bridge/launch/nextage_ros_bridge_real.launch
+++ b/nextage_ros_bridge/launch/nextage_ros_bridge_real.launch
@@ -5,17 +5,21 @@
+
+
+
+
diff --git a/nextage_ros_bridge/launch/nextage_ros_bridge_simulation.launch b/nextage_ros_bridge/launch/nextage_ros_bridge_simulation.launch
index dbba7ecc..fb08f240 100644
--- a/nextage_ros_bridge/launch/nextage_ros_bridge_simulation.launch
+++ b/nextage_ros_bridge/launch/nextage_ros_bridge_simulation.launch
@@ -6,6 +6,7 @@
+
@@ -20,6 +21,7 @@
+
diff --git a/nextage_ros_bridge/src/nextage_ros_bridge/nextage_client.py b/nextage_ros_bridge/src/nextage_ros_bridge/nextage_client.py
index 58a53813..5cc881b8 100755
--- a/nextage_ros_bridge/src/nextage_ros_bridge/nextage_client.py
+++ b/nextage_ros_bridge/src/nextage_ros_bridge/nextage_client.py
@@ -334,7 +334,7 @@ def getRTCList(self):
@return List of available components. Each element consists of a list
of abbreviated and full names of the component.
'''
- return [
+ rtclist = [
['seq', "SequencePlayer"],
['sh', "StateHolder"],
['fk', "ForwardKinematics"],
@@ -344,6 +344,22 @@ def getRTCList(self):
['ic', "ImpedanceController"],
['log', "DataLogger"]
]
+ if hasattr(self, 'rmfo'):
+ self.ms.load("RemoveForceSensorLinkOffset")
+ self.ms.load("AbsoluteForceSensor")
+ if "RemoveForceSensorLinkOffset" in self.ms.get_factory_names():
+ rtclist.append(['rmfo', "RemoveForceSensorLinkOffset"])
+ elif "AbsoluteForceSensor" in self.ms.get_factory_names():
+ rtclist.append(['rmfo', "AbsoluteForceSensor"])
+ else:
+ print "Component rmfo is not loadable."
+ # check if forcesensor iexists
+ if self.rh.port("lhsensor") or self.rh.port("rhsensor") :
+ print(self.configurator_name + "Setup with ImpedanceController")
+ else:
+ rtclist = [rtc for rtc in rtclist if not rtc[0] in ['ic', 'rmfo']]
+ print(self.configurator_name + "Setup without ImpedanceController")
+ return rtclist
def goInitial(self, tm=7, wait=True, init_pose_type=0):
'''
@@ -352,7 +368,19 @@ def goInitial(self, tm=7, wait=True, init_pose_type=0):
if not init_pose_type:
# Set the pose where eefs level with the tabletop by default.
init_pose_type = HIRONX.INITPOS_TYPE_EVEN
- return HIRONX.goInitial(self, tm, wait, init_pose_type)
+ ret = HIRONX.goInitial(self, tm, wait, init_pose_type)
+ if wait and self.ic and self.rmfo_svc and self.rh_svc:
+ i = 0
+ forces = self.rh_svc.getStatus().force
+ for sensor in ['rhsensor', 'lhsensor']:
+ force = forces[i]
+ (result, p) = self.rmfo_svc.getForceMomentOffsetParam(sensor)
+ p.force_offset=[force[0], force[1], force[2]]
+ p.moment_offset=[force[3], force[4], force[5]]
+ print(self.configurator_name + "Remove force offset {} with {}".format(sensor, force))
+ self.rmfo_svc.setForceMomentOffsetParam(sensor, p)
+ i = i + 1
+ return ret
def readDinGroup(self, ports, dumpFlag=True):
'''