diff --git a/VirtualRobot/Nodes/ForceTorqueSensor.cpp b/VirtualRobot/Nodes/ForceTorqueSensor.cpp index b3e573d6456305b1cdd501347f3039da61618b27..6785283a5990a3a782db1664ea302c6d4debba3c 100644 --- a/VirtualRobot/Nodes/ForceTorqueSensor.cpp +++ b/VirtualRobot/Nodes/ForceTorqueSensor.cpp @@ -62,9 +62,13 @@ namespace VirtualRobot } - SensorPtr ForceTorqueSensor::_clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr /*visualizationModel*/, float /*scaling*/) + SensorPtr ForceTorqueSensor::_clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr /*visualizationModel*/, float scaling) { - SensorPtr result(new ForceTorqueSensor(newRobotNode, name)); + THROW_VR_EXCEPTION_IF(scaling < 0, "Scaling must be >0"); + Eigen::Matrix4f rnt = rnTransformation; + rnt.block(0, 3, 3, 1) *= scaling; + ForceTorqueSensorPtr result(new ForceTorqueSensor(newRobotNode, name, rnt)); + result->updateSensors(forceTorqueValues); return result; } diff --git a/VirtualRobot/Nodes/ForceTorqueSensorFactory.cpp b/VirtualRobot/Nodes/ForceTorqueSensorFactory.cpp index c1e270c4aeac21f336d5e3d227a4e6b834d006c6..d1aba5324ae6c87122dce67daa5dcf0855224a4f 100644 --- a/VirtualRobot/Nodes/ForceTorqueSensorFactory.cpp +++ b/VirtualRobot/Nodes/ForceTorqueSensorFactory.cpp @@ -26,9 +26,9 @@ namespace VirtualRobot * \return instance of VirtualRobot::ForceTorqueSensor. */ SensorPtr ForceTorqueSensorFactory::createSensor(RobotNodePtr node, const std::string& name, VisualizationNodePtr /*visualization*/, - const Eigen::Matrix4f& /*rnTrafo*/) const + const Eigen::Matrix4f& rnTrafo) const { - SensorPtr Sensor(new ForceTorqueSensor(node, name)); + SensorPtr Sensor(new ForceTorqueSensor(node, name, rnTrafo)); return Sensor; }