From 9127dc62397fc8b90eee2e54f359b34902287e28 Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Sun, 16 Jun 2019 13:33:21 +0900 Subject: [PATCH 01/11] Added tactile sensor --- src/Body/BasicSensorSimulationHelper.cpp | 2 + src/Body/BasicSensorSimulationHelper.h | 3 ++ src/Body/CMakeLists.txt | 2 + src/Body/ForwardDynamicsABM.cpp | 52 ++++++++++++++++++++++++ src/Body/ForwardDynamicsABM.h | 1 + src/Body/VRMLBodyLoader.cpp | 13 ++++++ 6 files changed, 73 insertions(+) diff --git a/src/Body/BasicSensorSimulationHelper.cpp b/src/Body/BasicSensorSimulationHelper.cpp index 9bb9ce641..375eb2f08 100644 --- a/src/Body/BasicSensorSimulationHelper.cpp +++ b/src/Body/BasicSensorSimulationHelper.cpp @@ -80,10 +80,12 @@ void BasicSensorSimulationHelper::initialize(Body* body, double timeStep, const DeviceList<> devices = body->devices(); if(!devices.empty()){ forceSensors_.extractFrom(devices); + tactileSensors_.extractFrom(devices); rateGyroSensors_.extractFrom(devices); accelerationSensors_.extractFrom(devices); if(!forceSensors_.empty() || + !tactileSensors_.empty() || !rateGyroSensors_.empty() || !accelerationSensors_.empty()){ impl->initialize(body, timeStep, gravityAcceleration); diff --git a/src/Body/BasicSensorSimulationHelper.h b/src/Body/BasicSensorSimulationHelper.h index 6d15336a0..6bac4dbbb 100644 --- a/src/Body/BasicSensorSimulationHelper.h +++ b/src/Body/BasicSensorSimulationHelper.h @@ -8,6 +8,7 @@ #include "DeviceList.h" #include "ForceSensor.h" +#include "TactileSensor.h" #include "RateGyroSensor.h" #include "AccelerationSensor.h" #include "exportdecl.h" @@ -31,6 +32,7 @@ class CNOID_EXPORT BasicSensorSimulationHelper bool hasGyroOrAccelerationSensors() const { return !rateGyroSensors_.empty() || !accelerationSensors_.empty(); } const DeviceList& forceSensors() const { return forceSensors_; } + const DeviceList& tactileSensors() const { return tactileSensors_; } const DeviceList& rateGyroSensors() const { return rateGyroSensors_; } const DeviceList& accelerationSensors() const { return accelerationSensors_; } @@ -40,6 +42,7 @@ class CNOID_EXPORT BasicSensorSimulationHelper BasicSensorSimulationHelperImpl* impl; bool isActive_; DeviceList forceSensors_; + DeviceList tactileSensors_; DeviceList rateGyroSensors_; DeviceList accelerationSensors_; diff --git a/src/Body/CMakeLists.txt b/src/Body/CMakeLists.txt index 9dcd28c69..fbad83f42 100644 --- a/src/Body/CMakeLists.txt +++ b/src/Body/CMakeLists.txt @@ -36,6 +36,7 @@ set(sources Camera.cpp RangeCamera.cpp RangeSensor.cpp + TactileSensor.cpp Light.cpp PointLight.cpp SpotLight.cpp @@ -110,6 +111,7 @@ set(headers Camera.h RangeCamera.h RangeSensor.h + TactileSensor.h Light.h PointLight.h SpotLight.h diff --git a/src/Body/ForwardDynamicsABM.cpp b/src/Body/ForwardDynamicsABM.cpp index 058735e18..7cf7d0292 100644 --- a/src/Body/ForwardDynamicsABM.cpp +++ b/src/Body/ForwardDynamicsABM.cpp @@ -8,6 +8,8 @@ #include "LinkTraverse.h" #include +#include // Added by Rafa + using namespace std; using namespace cnoid; @@ -95,6 +97,9 @@ void ForwardDynamicsABM::calcMotionWithEulerMethod() if(!sensorHelper.forceSensors().empty()){ updateForceSensors(); } + if(!sensorHelper.tactileSensors().empty()){ + updateTactileSensors(); + } DyLink* root = body->rootLink(); @@ -177,6 +182,9 @@ void ForwardDynamicsABM::calcMotionWithRungeKuttaMethod() if(!sensorHelper.forceSensors().empty()){ updateForceSensors(); } + if(!sensorHelper.tactileSensors().empty()){ + updateTactileSensors(); + } integrateRungeKuttaOneStep(1.0 / 6.0, timeStep / 2.0); calcABMPhase1(false); @@ -498,6 +506,8 @@ void ForwardDynamicsABM::calcABMPhase3() void ForwardDynamicsABM::updateForceSensors() { + // std::cout << "Rafa, in ForwardDynamicsABM::updateForceSensors" << std::endl; + const DeviceList& sensors = sensorHelper.forceSensors(); for(size_t i=0; i < sensors.size(); ++i){ ForceSensor* sensor = sensors[i]; @@ -515,3 +525,45 @@ void ForwardDynamicsABM::updateForceSensors() sensor->notifyStateChange(); } } + + +void ForwardDynamicsABM::updateTactileSensors() +{ + const double EPSILON = 1e-6; + + const DeviceList& sensors = sensorHelper.tactileSensors(); + + for(size_t i=0; i < sensors.size(); ++i){ + + TactileSensor* sensor = sensors[i]; + + DyLink* link = static_cast(sensor->link()); + + std::cout << "Rafa, in ForwardDynamicsABM::updateTactileSensors, for sensor " << i << " link->name() = " << link->name() << std::endl; + + DyLink::ConstraintForceArray& forces = link->constraintForces(); + if(!forces.empty()){ + + std::cout << "Rafa, in ForwardDynamicsABM::updateTactileSensors, forces are not empty" << std::endl; + + for(size_t j=0; j < forces.size(); ++j){ + const DyLink::ConstraintForce& force = forces[i]; + + Vector3 p_surf = link->R().transpose() * (force.point - link->p()); + if (p_surf.z() < EPSILON) { + std::pair xy_f = std::make_pair(Vector2(p_surf.x(), p_surf.y()), force.force); + sensor->forceData().push_back(xy_f); + sensor->notifyStateChange(); + std::cout << "Rafa, in ForwardDynamicsABM::updateTactileSensors, in link " + << link->name() << " at point (" + << sensor->forceData().back().first.x() << ", " + << sensor->forceData().back().first.y() << ") there is a force (" + << sensor->forceData().back().second.x() << ", " + << sensor->forceData().back().second.y() << ", " + << sensor->forceData().back().second.z() << ")" + << std::endl; + } + } + } + } +} diff --git a/src/Body/ForwardDynamicsABM.h b/src/Body/ForwardDynamicsABM.h index 7166d8c90..7231c85bf 100644 --- a/src/Body/ForwardDynamicsABM.h +++ b/src/Body/ForwardDynamicsABM.h @@ -52,6 +52,7 @@ class CNOID_EXPORT ForwardDynamicsABM : public ForwardDynamics inline void calcABMLastHalf(); void updateForceSensors(); + void updateTactileSensors(); // Buffers for the Runge Kutta Method Position T0; diff --git a/src/Body/VRMLBodyLoader.cpp b/src/Body/VRMLBodyLoader.cpp index 97f29643f..069422d21 100644 --- a/src/Body/VRMLBodyLoader.cpp +++ b/src/Body/VRMLBodyLoader.cpp @@ -11,6 +11,7 @@ #include "Camera.h" #include "RangeCamera.h" #include "RangeSensor.h" +#include "TactileSensor.h" #include "PointLight.h" #include "SpotLight.h" #include @@ -116,6 +117,7 @@ class VRMLBodyLoaderImpl static AccelerationSensorPtr createAccelerationSensor(VRMLProtoInstance* node); static CameraPtr createCamera(VRMLProtoInstance* node); static RangeSensorPtr createRangeSensor(VRMLProtoInstance* node); + static TactileSensorPtr createTactileSensor(VRMLProtoInstance* node); static void readLightDeviceCommonParameters(Light& light, VRMLProtoInstance* node); static SpotLightPtr createSpotLight(VRMLProtoInstance* node); void setExtraJoints(); @@ -298,6 +300,7 @@ VRMLBodyLoaderImpl::VRMLBodyLoaderImpl() protoInfoMap["PressureSensor"] = ProtoInfo(PROTO_DEVICE, &VRMLBodyLoaderImpl::checkSensorProtoCommon); protoInfoMap["VisionSensor"] = ProtoInfo(PROTO_DEVICE, &VRMLBodyLoaderImpl::checkVisionSensorProto); protoInfoMap["RangeSensor"] = ProtoInfo(PROTO_DEVICE, &VRMLBodyLoaderImpl::checkRangeSensorProto); + protoInfoMap["TactileSensor"] = ProtoInfo(PROTO_DEVICE, &VRMLBodyLoaderImpl::checkSensorProtoCommon); protoInfoMap["SpotLightDevice"] = ProtoInfo(PROTO_DEVICE, &VRMLBodyLoaderImpl::checkSpotLightDeviceProto); protoInfoMap["ExtraJoint"] = ProtoInfo(PROTO_EXTRAJOINT, &VRMLBodyLoaderImpl::checkExtraJointProto); } @@ -311,6 +314,7 @@ VRMLBodyLoaderImpl::VRMLBodyLoaderImpl() //sensorTypeMap["TorqueSensor"] = Sensor::TORQUE; deviceFactories["RangeSensor"] = &VRMLBodyLoaderImpl::createRangeSensor; deviceFactories["VisionSensor"] = &VRMLBodyLoaderImpl::createCamera; + deviceFactories["TactileSensor"] = &VRMLBodyLoaderImpl::createTactileSensor; deviceFactories["SpotLightDevice"] = &VRMLBodyLoaderImpl::createSpotLight; } } @@ -1187,6 +1191,15 @@ RangeSensorPtr VRMLBodyLoaderImpl::createRangeSensor(VRMLProtoInstance* node) } +TactileSensorPtr VRMLBodyLoaderImpl::createTactileSensor(VRMLProtoInstance* node) +{ + TactileSensorPtr sensor = new TactileSensor(); + readDeviceCommonParameters(*sensor, node); + + return sensor; +} + + void VRMLBodyLoaderImpl::readLightDeviceCommonParameters(Light& light, VRMLProtoInstance* node) { readDeviceCommonParameters(light, node); From 04eb7986d68ad9dbcc4c2e4ed8aa90edd48e24cb Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Mon, 17 Jun 2019 12:18:19 +0900 Subject: [PATCH 02/11] Had forgotten to add TactileSensor --- src/Body/TactileSensor.cpp | 86 ++++++++++++++++++++++++++++++++++++++ src/Body/TactileSensor.h | 47 +++++++++++++++++++++ 2 files changed, 133 insertions(+) create mode 100644 src/Body/TactileSensor.cpp create mode 100644 src/Body/TactileSensor.h diff --git a/src/Body/TactileSensor.cpp b/src/Body/TactileSensor.cpp new file mode 100644 index 000000000..f17e84f2e --- /dev/null +++ b/src/Body/TactileSensor.cpp @@ -0,0 +1,86 @@ +/** + \file + \author Rafael Cisneros +*/ + +#include "TactileSensor.h" + +using namespace cnoid; + + +TactileSensor::TactileSensor() +{ + forceData_ = std::make_shared(); +} + + +TactileSensor::TactileSensor(const TactileSensor& org, bool copyStateOnly) + : Device(org, copyStateOnly) +{ + copyStateFrom(org); +} + + +const char* TactileSensor::typeName() +{ + return "TactileSensor"; +} + + +void TactileSensor::copyStateFrom(const TactileSensor& other) +{ + forceData_ = other.forceData_; +} + + +void TactileSensor::copyStateFrom(const DeviceState& other) +{ + if (typeid(other) != typeid(TactileSensor)) + throw std::invalid_argument("Type mismatch in the Device::copyStateFrom function"); + + copyStateFrom(static_cast(other)); +} + + +Device* TactileSensor::clone() const +{ + return new TactileSensor(*this, false); +} + + +DeviceState* TactileSensor::cloneState() const +{ + return new TactileSensor(*this, true); +} + + +void TactileSensor::forEachActualType(std::function func) +{ + if (!func(typeid(TactileSensor))) + Device::forEachActualType(func); +} + + +void TactileSensor::clearState() +{ + //forceData_ = std::make_shared(std::make_pair(Vector2::Zero(), Vector3::Zero())); + forceData_->clear(); +} + + +int TactileSensor::stateSize() const +{ + return 0; +} + + +const double* TactileSensor::readState(const double* buf) +{ + return buf; +} + + +double* TactileSensor::writeState(double* out_buf) const +{ + return out_buf; +} diff --git a/src/Body/TactileSensor.h b/src/Body/TactileSensor.h new file mode 100644 index 000000000..fc68307d8 --- /dev/null +++ b/src/Body/TactileSensor.h @@ -0,0 +1,47 @@ +/** + \file + \author Rafael Cisneros +*/ + +#ifndef CNOID_BODY_TACTILE_SENSOR_H +#define CNOID_BODY_TACTILE_SENSOR_H + +#include "Device.h" +#include +#include "exportdecl.h" + +namespace cnoid { + +class CNOID_EXPORT TactileSensor : public Device +{ + public: + + TactileSensor(); + TactileSensor(const TactileSensor& org, bool copyStateOnly = false); + + virtual const char* typeName() override; + void copyStateFrom(const TactileSensor& other); + virtual void copyStateFrom(const DeviceState& other) override; + virtual Device* clone() const override; + virtual DeviceState* cloneState() const override; + virtual void forEachActualType(std::function func) override; + virtual void clearState() override; + virtual int stateSize() const override; + virtual const double* readState(const double* buf) override; + virtual double* writeState(double* out_buf) const override; + + typedef std::vector< std::pair > ForceData; + + const ForceData& forceData() const { return *forceData_; } + ForceData& forceData() { return *forceData_; } + + private: + + std::shared_ptr forceData_; +}; + + typedef ref_ptr TactileSensorPtr; + +} + +#endif From 6b23da78475bf350cc5e54e5b2802173b4a8eeb2 Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Wed, 19 Jun 2019 19:17:18 +0900 Subject: [PATCH 03/11] Changes to OpenRTMPlugin for the Tactile Sensor and some pending debugging comments related to the initial velocity --- include/cnoid/BasicSensors | 1 + src/Body/Body.cpp | 2 ++ src/Body/ForceSensor.cpp | 4 ++++ src/Body/ForwardDynamicsABM.cpp | 4 ++-- src/Body/TactileSensor.cpp | 5 ++++- src/OpenRTMPlugin/deprecated/BodyRTCItem.cpp | 19 +++++++++++++++++++ src/OpenRTMPlugin/deprecated/BodyRTCItem.h | 1 + src/OpenRTMPlugin/deprecated/BridgeConf.h | 1 + .../deprecated/VirtualRobotPortHandler.cpp | 2 ++ .../deprecated/VirtualRobotRTC.cpp | 3 +++ 10 files changed, 39 insertions(+), 3 deletions(-) diff --git a/include/cnoid/BasicSensors b/include/cnoid/BasicSensors index 8598a9c31..8b670f752 100644 --- a/include/cnoid/BasicSensors +++ b/include/cnoid/BasicSensors @@ -1,3 +1,4 @@ #include "src/Body/ForceSensor.h" +#include "src/Body/TactileSensor.h" #include "src/Body/RateGyroSensor.h" #include "src/Body/AccelerationSensor.h" diff --git a/src/Body/Body.cpp b/src/Body/Body.cpp index 2095aaa63..fc13b184b 100644 --- a/src/Body/Body.cpp +++ b/src/Body/Body.cpp @@ -476,6 +476,8 @@ void Body::initializePosition() void Body::initializeState() { + std::cout << "Rafa, in Body::initializeState" << std::endl; + for(auto& link : linkTraverse_){ link->initializeState(); } diff --git a/src/Body/ForceSensor.cpp b/src/Body/ForceSensor.cpp index cb55bda97..467a8c069 100644 --- a/src/Body/ForceSensor.cpp +++ b/src/Body/ForceSensor.cpp @@ -5,6 +5,8 @@ #include "ForceSensor.h" +#include // Added by Rafa + using namespace cnoid; @@ -94,6 +96,8 @@ const double* ForceSensor::readState(const double* buf) double* ForceSensor::writeState(double* out_buf) const { + std::cout << "Rafa, in ForceSensor::writeState" << std::endl; + Eigen::Map(out_buf) << F_; return out_buf + 6; } diff --git a/src/Body/ForwardDynamicsABM.cpp b/src/Body/ForwardDynamicsABM.cpp index 7cf7d0292..8ce8d5ab7 100644 --- a/src/Body/ForwardDynamicsABM.cpp +++ b/src/Body/ForwardDynamicsABM.cpp @@ -539,12 +539,12 @@ void ForwardDynamicsABM::updateTactileSensors() DyLink* link = static_cast(sensor->link()); - std::cout << "Rafa, in ForwardDynamicsABM::updateTactileSensors, for sensor " << i << " link->name() = " << link->name() << std::endl; + // std::cout << "Rafa, in ForwardDynamicsABM::updateTactileSensors, for sensor " << i << " link->name() = " << link->name() << std::endl; DyLink::ConstraintForceArray& forces = link->constraintForces(); if(!forces.empty()){ - std::cout << "Rafa, in ForwardDynamicsABM::updateTactileSensors, forces are not empty" << std::endl; + // std::cout << "Rafa, in ForwardDynamicsABM::updateTactileSensors, forces are not empty" << std::endl; for(size_t j=0; j < forces.size(); ++j){ const DyLink::ConstraintForce& force = forces[i]; diff --git a/src/Body/TactileSensor.cpp b/src/Body/TactileSensor.cpp index f17e84f2e..d376de9e5 100644 --- a/src/Body/TactileSensor.cpp +++ b/src/Body/TactileSensor.cpp @@ -5,6 +5,8 @@ #include "TactileSensor.h" +#include // Rafa, this is temporal + using namespace cnoid; @@ -63,13 +65,14 @@ void TactileSensor::forEachActualType(std::function(std::make_pair(Vector2::Zero(), Vector3::Zero())); forceData_->clear(); } int TactileSensor::stateSize() const { + std::cout << "Rafa, in TactileSensor::stateSize, forceData_->size() = " << forceData_->size() << std::endl; + return 0; } diff --git a/src/OpenRTMPlugin/deprecated/BodyRTCItem.cpp b/src/OpenRTMPlugin/deprecated/BodyRTCItem.cpp index a0984558a..c8f82c23a 100644 --- a/src/OpenRTMPlugin/deprecated/BodyRTCItem.cpp +++ b/src/OpenRTMPlugin/deprecated/BodyRTCItem.cpp @@ -61,6 +61,7 @@ class BodyRTCItemImpl BodyPtr simulationBody; DeviceList forceSensors_; + DeviceList tactileSensors_; DeviceList gyroSensors_; DeviceList accelSensors_; double timeStep_; @@ -354,6 +355,16 @@ void BodyRTCItemImpl::setdefaultPort(BodyPtr body) outPortInfoMap.insert(make_pair(portInfo.portName, portInfo)); } } + for(size_t i=0; i < tactileSensors_.size(); ++i){ + if(Device* sensor = tactileSensors_[i]){ + portInfo.dataTypeId = TACTILE_SENSOR; + portInfo.dataOwnerNames.clear(); + portInfo.dataOwnerNames.push_back(sensor->name()); + portInfo.portName = sensor->name(); + portInfo.stepTime = 0; + outPortInfoMap.insert(make_pair(portInfo.portName, portInfo)); + } + } for(size_t i=0; i < gyroSensors_.size(); ++i){ if(Device* sensor = gyroSensors_[i]){ portInfo.dataTypeId = RATE_GYRO_SENSOR; @@ -399,6 +410,7 @@ void BodyRTCItemImpl::onPositionChanged() Body* body = ownerBodyItem->body(); if(bodyName != body->name()){ forceSensors_ = body->devices().getSortedById(); + tactileSensors_ = body->devices().getSortedById(); gyroSensors_ = body->devices().getSortedById(); accelSensors_ = body->devices().getSortedById(); bodyName = body->name(); @@ -446,6 +458,7 @@ bool BodyRTCItemImpl::initialize(ControllerIO* io) } forceSensors_ = simulationBody->devices().getSortedById(); + tactileSensors_ = simulationBody->devices().getSortedById(); gyroSensors_ = simulationBody->devices().getSortedById(); accelSensors_ = simulationBody->devices().getSortedById(); @@ -599,6 +612,12 @@ const DeviceList& BodyRTCItem::forceSensors() const } +const DeviceList& BodyRTCItem::tactileSensors() const +{ + return impl->tactileSensors_; +} + + const DeviceList& BodyRTCItem::rateGyroSensors() const { return impl->gyroSensors_; diff --git a/src/OpenRTMPlugin/deprecated/BodyRTCItem.h b/src/OpenRTMPlugin/deprecated/BodyRTCItem.h index d6a19918d..5b16771f9 100644 --- a/src/OpenRTMPlugin/deprecated/BodyRTCItem.h +++ b/src/OpenRTMPlugin/deprecated/BodyRTCItem.h @@ -34,6 +34,7 @@ class CNOID_EXPORT BodyRTCItem : public ControllerItem const Body* body() const; const DeviceList& forceSensors() const; + const DeviceList& tactileSensors() const; const DeviceList& rateGyroSensors() const; const DeviceList& accelerationSensors() const; diff --git a/src/OpenRTMPlugin/deprecated/BridgeConf.h b/src/OpenRTMPlugin/deprecated/BridgeConf.h index b167d5d65..21b11014b 100644 --- a/src/OpenRTMPlugin/deprecated/BridgeConf.h +++ b/src/OpenRTMPlugin/deprecated/BridgeConf.h @@ -34,6 +34,7 @@ enum DataTypeId { RATE_GYRO_SENSOR, ACCELERATION_SENSOR, RANGE_SENSOR, + TACTILE_SENSOR, CONSTRAINT_FORCE, RATE_GYRO_SENSOR2, ACCELERATION_SENSOR2, diff --git a/src/OpenRTMPlugin/deprecated/VirtualRobotPortHandler.cpp b/src/OpenRTMPlugin/deprecated/VirtualRobotPortHandler.cpp index b4b49a1a1..25c1a4214 100644 --- a/src/OpenRTMPlugin/deprecated/VirtualRobotPortHandler.cpp +++ b/src/OpenRTMPlugin/deprecated/VirtualRobotPortHandler.cpp @@ -327,11 +327,13 @@ void SensorDataOutPortHandler::inputDataFromSimulator(BodyRTCItem* bodyRTC) auto body = bodyRTC->body(); if(!sensorNames.empty()){ if(Device* sensor = body->findDevice(sensorNames[0])){ + std::cout << "Rafa, in SensorDataOutPortHandler::inputDataFromSimulator, first check, sensor->typeName = " << sensor->typeName() << std::endl; const int dataSize = sensor->stateSize(); value.data.length(dataSize); if(dataSize > 0){ for(size_t i=0; i < sensorNames.size(); ++i){ if(Device* sensor = body->findDevice(sensorNames[i])){ + std::cout << "Rafa, in SensorDataOutPortHandler::inputDataFromSimulator, if dataSize > 0, sensor->typeName = " << sensor->typeName() << std::endl; sensor->writeState(&value.data[i * dataSize]); } } diff --git a/src/OpenRTMPlugin/deprecated/VirtualRobotRTC.cpp b/src/OpenRTMPlugin/deprecated/VirtualRobotRTC.cpp index 39bfd94e4..2a4c6a397 100644 --- a/src/OpenRTMPlugin/deprecated/VirtualRobotRTC.cpp +++ b/src/OpenRTMPlugin/deprecated/VirtualRobotRTC.cpp @@ -106,6 +106,7 @@ bool VirtualRobotRTC::createOutPortHandler(PortInfo& portInfo) case JOINT_ACCELERATION: case JOINT_TORQUE: case FORCE_SENSOR: + case TACTILE_SENSOR: case RATE_GYRO_SENSOR: case ACCELERATION_SENSOR: ret = registerOutPortHandler(new SensorStateOutPortHandler(portInfo)); @@ -142,6 +143,8 @@ bool VirtualRobotRTC::createOutPortHandler(PortInfo& portInfo) break; case FORCE_SENSOR: + case TACTILE_SENSOR: + std::cout << "Rafa, in VirtualRobotRTC::createOutPortHandler, TACTILE_SENSOR" << std::endl; case RATE_GYRO_SENSOR: case ACCELERATION_SENSOR: ret = registerOutPortHandler(new SensorDataOutPortHandler(portInfo)); From 165df1bd390714733db45c58bec8acf44202105b Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Thu, 20 Jun 2019 19:46:23 +0900 Subject: [PATCH 04/11] Managed to send the tactile information through the corresponding RTC outports --- src/Body/ForceSensor.cpp | 2 +- src/Body/ForwardDynamicsABM.cpp | 9 +++++---- src/Body/TactileSensor.cpp | 9 ++++++++- src/OpenRTMPlugin/deprecated/BridgeConf.cpp | 1 + src/OpenRTMPlugin/deprecated/VirtualRobotPortHandler.cpp | 2 +- src/OpenRTMPlugin/deprecated/VirtualRobotRTC.cpp | 1 - 6 files changed, 16 insertions(+), 8 deletions(-) diff --git a/src/Body/ForceSensor.cpp b/src/Body/ForceSensor.cpp index 467a8c069..33f8737df 100644 --- a/src/Body/ForceSensor.cpp +++ b/src/Body/ForceSensor.cpp @@ -96,7 +96,7 @@ const double* ForceSensor::readState(const double* buf) double* ForceSensor::writeState(double* out_buf) const { - std::cout << "Rafa, in ForceSensor::writeState" << std::endl; + std::cout << "Rafa, in ForceSensor::writeState" << std::endl; Eigen::Map(out_buf) << F_; return out_buf + 6; diff --git a/src/Body/ForwardDynamicsABM.cpp b/src/Body/ForwardDynamicsABM.cpp index 8ce8d5ab7..5f270c57b 100644 --- a/src/Body/ForwardDynamicsABM.cpp +++ b/src/Body/ForwardDynamicsABM.cpp @@ -545,13 +545,14 @@ void ForwardDynamicsABM::updateTactileSensors() if(!forces.empty()){ // std::cout << "Rafa, in ForwardDynamicsABM::updateTactileSensors, forces are not empty" << std::endl; + + sensor->forceData().clear(); for(size_t j=0; j < forces.size(); ++j){ - const DyLink::ConstraintForce& force = forces[i]; - - Vector3 p_surf = link->R().transpose() * (force.point - link->p()); + const DyLink::ConstraintForce& force_surf = forces[j]; + Vector3 p_surf = link->R().transpose() * (force_surf.point - link->p()); if (p_surf.z() < EPSILON) { - std::pair xy_f = std::make_pair(Vector2(p_surf.x(), p_surf.y()), force.force); + std::pair xy_f = std::make_pair(Vector2(p_surf.x(), p_surf.y()), link->R().transpose() * force_surf.force); sensor->forceData().push_back(xy_f); sensor->notifyStateChange(); std::cout << "Rafa, in ForwardDynamicsABM::updateTactileSensors, in link " diff --git a/src/Body/TactileSensor.cpp b/src/Body/TactileSensor.cpp index d376de9e5..c90d42025 100644 --- a/src/Body/TactileSensor.cpp +++ b/src/Body/TactileSensor.cpp @@ -73,7 +73,7 @@ int TactileSensor::stateSize() const { std::cout << "Rafa, in TactileSensor::stateSize, forceData_->size() = " << forceData_->size() << std::endl; - return 0; + return forceData_->size() * 5; } @@ -85,5 +85,12 @@ const double* TactileSensor::readState(const double* buf) double* TactileSensor::writeState(double* out_buf) const { + for (size_t i = 0; i < forceData_->size(); i++) { + Eigen::Map(out_buf) << (*forceData_)[i].first; + out_buf = out_buf + 2; + Eigen::Map(out_buf) << (*forceData_)[i].second; + out_buf = out_buf + 3; + } + return out_buf; } diff --git a/src/OpenRTMPlugin/deprecated/BridgeConf.cpp b/src/OpenRTMPlugin/deprecated/BridgeConf.cpp index 6ce08ec76..0c139e0d8 100644 --- a/src/OpenRTMPlugin/deprecated/BridgeConf.cpp +++ b/src/OpenRTMPlugin/deprecated/BridgeConf.cpp @@ -108,6 +108,7 @@ void BridgeConf::initLabelToDataTypeMap() m["ABS_VELOCITY"] = ABS_VELOCITY; m["ABS_ACCELERATION"] = ABS_ACCELERATION; m["FORCE_SENSOR"] = FORCE_SENSOR; + m["TACTILE_SENSOR"] = TACTILE_SENSOR; m["RATE_GYRO_SENSOR"] = RATE_GYRO_SENSOR; m["ACCELERATION_SENSOR"] = ACCELERATION_SENSOR; m["RANGE_SENSOR"] = RANGE_SENSOR; diff --git a/src/OpenRTMPlugin/deprecated/VirtualRobotPortHandler.cpp b/src/OpenRTMPlugin/deprecated/VirtualRobotPortHandler.cpp index 25c1a4214..f897b956e 100644 --- a/src/OpenRTMPlugin/deprecated/VirtualRobotPortHandler.cpp +++ b/src/OpenRTMPlugin/deprecated/VirtualRobotPortHandler.cpp @@ -312,7 +312,7 @@ SensorDataOutPortHandler::SensorDataOutPortHandler(PortInfo& info) outPort(info.portName.c_str(), value), sensorNames(info.dataOwnerNames) { - + std::cout << "Rafa, in SensorDataOutPortHandler::SensorDataOutPortHandler, sensorNames.size() = " << sensorNames.size() << std::endl; } diff --git a/src/OpenRTMPlugin/deprecated/VirtualRobotRTC.cpp b/src/OpenRTMPlugin/deprecated/VirtualRobotRTC.cpp index 2a4c6a397..0d6fd09c5 100644 --- a/src/OpenRTMPlugin/deprecated/VirtualRobotRTC.cpp +++ b/src/OpenRTMPlugin/deprecated/VirtualRobotRTC.cpp @@ -144,7 +144,6 @@ bool VirtualRobotRTC::createOutPortHandler(PortInfo& portInfo) case FORCE_SENSOR: case TACTILE_SENSOR: - std::cout << "Rafa, in VirtualRobotRTC::createOutPortHandler, TACTILE_SENSOR" << std::endl; case RATE_GYRO_SENSOR: case ACCELERATION_SENSOR: ret = registerOutPortHandler(new SensorDataOutPortHandler(portInfo)); From f39e79c011ea8357bd9ef1173ac07ce1e7a8e2aa Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Mon, 24 Jun 2019 22:55:13 +0900 Subject: [PATCH 05/11] Temporal implementation of only outputting points from the tactile sensor --- .../ContactForceExtractionSamplePlugin.cpp | 8 ++++---- src/Body/ForceSensor.cpp | 2 +- src/Body/ForwardDynamicsABM.cpp | 13 ++++++++++++- src/Body/TactileSensor.cpp | 12 +++++++----- src/Body/TactileSensor.h | 3 ++- .../deprecated/VirtualRobotPortHandler.cpp | 6 +++--- 6 files changed, 29 insertions(+), 15 deletions(-) diff --git a/sample/ContactForceExtraction/ContactForceExtractionSamplePlugin.cpp b/sample/ContactForceExtraction/ContactForceExtractionSamplePlugin.cpp index f02ea14e7..2137e4d46 100644 --- a/sample/ContactForceExtraction/ContactForceExtractionSamplePlugin.cpp +++ b/sample/ContactForceExtraction/ContactForceExtractionSamplePlugin.cpp @@ -116,16 +116,16 @@ void ContactForceExtractorItem::extractBodyContactPoints(DyBody* body, ostream& DyLink::ConstraintForceArray& forces = link->constraintForces(); if(!forces.empty()){ if(!put){ - os << body->name() << ":\n"; + // os << body->name() << ":\n"; // Rafa commented this put = true; } - os << " " << link->name() << ":\n"; + // os << " " << link->name() << ":\n"; // Rafa commented this for(size_t i=0; i < forces.size(); ++i){ const DyLink::ConstraintForce& force = forces[i]; const Vector3& p = force.point; const Vector3& f = force.force; - os << " point(" << p.x() << ", " << p.y() << ", " << p.z() - << "), force(" << f.x() << ", " << f.y() << ", " << f.z() << ")\n"; + // os << " point(" << p.x() << ", " << p.y() << ", " << p.z() + // << "), force(" << f.x() << ", " << f.y() << ", " << f.z() << ")\n"; // Rafa commented this } } } diff --git a/src/Body/ForceSensor.cpp b/src/Body/ForceSensor.cpp index 33f8737df..e60f4081c 100644 --- a/src/Body/ForceSensor.cpp +++ b/src/Body/ForceSensor.cpp @@ -96,7 +96,7 @@ const double* ForceSensor::readState(const double* buf) double* ForceSensor::writeState(double* out_buf) const { - std::cout << "Rafa, in ForceSensor::writeState" << std::endl; + // std::cout << "Rafa, in ForceSensor::writeState" << std::endl; Eigen::Map(out_buf) << F_; return out_buf + 6; diff --git a/src/Body/ForwardDynamicsABM.cpp b/src/Body/ForwardDynamicsABM.cpp index 5f270c57b..cb33f2635 100644 --- a/src/Body/ForwardDynamicsABM.cpp +++ b/src/Body/ForwardDynamicsABM.cpp @@ -552,9 +552,12 @@ void ForwardDynamicsABM::updateTactileSensors() const DyLink::ConstraintForce& force_surf = forces[j]; Vector3 p_surf = link->R().transpose() * (force_surf.point - link->p()); if (p_surf.z() < EPSILON) { - std::pair xy_f = std::make_pair(Vector2(p_surf.x(), p_surf.y()), link->R().transpose() * force_surf.force); + // std::pair xy_f = std::make_pair(Vector2(p_surf.x(), p_surf.y()), link->R().transpose() * force_surf.force); // Rafa commented this + Vector2 xy_f = Vector2(p_surf.x(), p_surf.y()); // Rafa, temporal implementation sensor->forceData().push_back(xy_f); sensor->notifyStateChange(); + // Rafa commented this + /* std::cout << "Rafa, in ForwardDynamicsABM::updateTactileSensors, in link " << link->name() << " at point (" << sensor->forceData().back().first.x() << ", " @@ -563,6 +566,14 @@ void ForwardDynamicsABM::updateTactileSensors() << sensor->forceData().back().second.y() << ", " << sensor->forceData().back().second.z() << ")" << std::endl; + */ + /* + std::cout << "Rafa, in ForwardDynamicsABM::updateTactileSensors, in link " + << link->name() << " at point (" + << sensor->forceData().back().x() << ", " + << sensor->forceData().back().y() << ") there is a force" + << std::endl; // Rafa, temporal implementation + */ } } } diff --git a/src/Body/TactileSensor.cpp b/src/Body/TactileSensor.cpp index c90d42025..f94b100c0 100644 --- a/src/Body/TactileSensor.cpp +++ b/src/Body/TactileSensor.cpp @@ -71,9 +71,10 @@ void TactileSensor::clearState() int TactileSensor::stateSize() const { - std::cout << "Rafa, in TactileSensor::stateSize, forceData_->size() = " << forceData_->size() << std::endl; + // std::cout << "Rafa, in TactileSensor::stateSize, forceData_->size() = " << forceData_->size() << std::endl; - return forceData_->size() * 5; + // return forceData_->size() * 5; // Rafa commented this + return forceData_->size() * 2; // Rafa, temporal implementation } @@ -86,10 +87,11 @@ const double* TactileSensor::readState(const double* buf) double* TactileSensor::writeState(double* out_buf) const { for (size_t i = 0; i < forceData_->size(); i++) { - Eigen::Map(out_buf) << (*forceData_)[i].first; + // Eigen::Map(out_buf) << (*forceData_)[i].first; // Rafa commented this + Eigen::Map(out_buf) << (*forceData_)[i]; // Rafa, temporal implementation out_buf = out_buf + 2; - Eigen::Map(out_buf) << (*forceData_)[i].second; - out_buf = out_buf + 3; + // Eigen::Map(out_buf) << (*forceData_)[i].second; // Rafa commented this + // out_buf = out_buf + 3; // Rafa commented this } return out_buf; diff --git a/src/Body/TactileSensor.h b/src/Body/TactileSensor.h index fc68307d8..8200483bb 100644 --- a/src/Body/TactileSensor.h +++ b/src/Body/TactileSensor.h @@ -30,7 +30,8 @@ class CNOID_EXPORT TactileSensor : public Device virtual const double* readState(const double* buf) override; virtual double* writeState(double* out_buf) const override; - typedef std::vector< std::pair > ForceData; + // typedef std::vector< std::pair > ForceData; // Rafa commented this + typedef std::vector ForceData; // Rafa, temporal implementation const ForceData& forceData() const { return *forceData_; } ForceData& forceData() { return *forceData_; } diff --git a/src/OpenRTMPlugin/deprecated/VirtualRobotPortHandler.cpp b/src/OpenRTMPlugin/deprecated/VirtualRobotPortHandler.cpp index f897b956e..c887b1875 100644 --- a/src/OpenRTMPlugin/deprecated/VirtualRobotPortHandler.cpp +++ b/src/OpenRTMPlugin/deprecated/VirtualRobotPortHandler.cpp @@ -312,7 +312,7 @@ SensorDataOutPortHandler::SensorDataOutPortHandler(PortInfo& info) outPort(info.portName.c_str(), value), sensorNames(info.dataOwnerNames) { - std::cout << "Rafa, in SensorDataOutPortHandler::SensorDataOutPortHandler, sensorNames.size() = " << sensorNames.size() << std::endl; + // std::cout << "Rafa, in SensorDataOutPortHandler::SensorDataOutPortHandler, sensorNames.size() = " << sensorNames.size() << std::endl; } @@ -327,13 +327,13 @@ void SensorDataOutPortHandler::inputDataFromSimulator(BodyRTCItem* bodyRTC) auto body = bodyRTC->body(); if(!sensorNames.empty()){ if(Device* sensor = body->findDevice(sensorNames[0])){ - std::cout << "Rafa, in SensorDataOutPortHandler::inputDataFromSimulator, first check, sensor->typeName = " << sensor->typeName() << std::endl; + // std::cout << "Rafa, in SensorDataOutPortHandler::inputDataFromSimulator, first check, sensor->typeName = " << sensor->typeName() << std::endl; const int dataSize = sensor->stateSize(); value.data.length(dataSize); if(dataSize > 0){ for(size_t i=0; i < sensorNames.size(); ++i){ if(Device* sensor = body->findDevice(sensorNames[i])){ - std::cout << "Rafa, in SensorDataOutPortHandler::inputDataFromSimulator, if dataSize > 0, sensor->typeName = " << sensor->typeName() << std::endl; + // std::cout << "Rafa, in SensorDataOutPortHandler::inputDataFromSimulator, if dataSize > 0, sensor->typeName = " << sensor->typeName() << std::endl; sensor->writeState(&value.data[i * dataSize]); } } From fb1ea8454573c738af6103883f4aed07a9ae882b Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Sun, 30 Jun 2019 22:58:13 +0900 Subject: [PATCH 06/11] Disabled the temporal implementation of the tactile sensor --- src/Body/ForwardDynamicsABM.cpp | 5 +++-- src/Body/TactileSensor.cpp | 12 ++++++------ src/Body/TactileSensor.h | 4 ++-- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/src/Body/ForwardDynamicsABM.cpp b/src/Body/ForwardDynamicsABM.cpp index cb33f2635..68aa21aae 100644 --- a/src/Body/ForwardDynamicsABM.cpp +++ b/src/Body/ForwardDynamicsABM.cpp @@ -551,9 +551,10 @@ void ForwardDynamicsABM::updateTactileSensors() for(size_t j=0; j < forces.size(); ++j){ const DyLink::ConstraintForce& force_surf = forces[j]; Vector3 p_surf = link->R().transpose() * (force_surf.point - link->p()); + Vector3 f_surf = link->R().transpose() * force_surf.force; if (p_surf.z() < EPSILON) { - // std::pair xy_f = std::make_pair(Vector2(p_surf.x(), p_surf.y()), link->R().transpose() * force_surf.force); // Rafa commented this - Vector2 xy_f = Vector2(p_surf.x(), p_surf.y()); // Rafa, temporal implementation + std::pair xy_f = std::make_pair(Vector2(p_surf.x(), p_surf.y()), f_surf); + // Vector2 xy_f = Vector2(p_surf.x(), p_surf.y()); // Rafa, temporal implementation sensor->forceData().push_back(xy_f); sensor->notifyStateChange(); // Rafa commented this diff --git a/src/Body/TactileSensor.cpp b/src/Body/TactileSensor.cpp index f94b100c0..ea1c71be3 100644 --- a/src/Body/TactileSensor.cpp +++ b/src/Body/TactileSensor.cpp @@ -73,8 +73,8 @@ int TactileSensor::stateSize() const { // std::cout << "Rafa, in TactileSensor::stateSize, forceData_->size() = " << forceData_->size() << std::endl; - // return forceData_->size() * 5; // Rafa commented this - return forceData_->size() * 2; // Rafa, temporal implementation + return forceData_->size() * 5; + // return forceData_->size() * 2; // Rafa, temporal implementation } @@ -87,11 +87,11 @@ const double* TactileSensor::readState(const double* buf) double* TactileSensor::writeState(double* out_buf) const { for (size_t i = 0; i < forceData_->size(); i++) { - // Eigen::Map(out_buf) << (*forceData_)[i].first; // Rafa commented this - Eigen::Map(out_buf) << (*forceData_)[i]; // Rafa, temporal implementation + Eigen::Map(out_buf) << (*forceData_)[i].first; + //Eigen::Map(out_buf) << (*forceData_)[i]; // Rafa, temporal implementation out_buf = out_buf + 2; - // Eigen::Map(out_buf) << (*forceData_)[i].second; // Rafa commented this - // out_buf = out_buf + 3; // Rafa commented this + Eigen::Map(out_buf) << (*forceData_)[i].second; + out_buf = out_buf + 3; } return out_buf; diff --git a/src/Body/TactileSensor.h b/src/Body/TactileSensor.h index 8200483bb..4853e8d32 100644 --- a/src/Body/TactileSensor.h +++ b/src/Body/TactileSensor.h @@ -30,8 +30,8 @@ class CNOID_EXPORT TactileSensor : public Device virtual const double* readState(const double* buf) override; virtual double* writeState(double* out_buf) const override; - // typedef std::vector< std::pair > ForceData; // Rafa commented this - typedef std::vector ForceData; // Rafa, temporal implementation + typedef std::vector< std::pair > ForceData; + // typedef std::vector ForceData; // Rafa, temporal implementation const ForceData& forceData() const { return *forceData_; } ForceData& forceData() { return *forceData_; } From d6690aa1a938ba97ac0194942b9120bbab6efbbd Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Wed, 9 Oct 2019 01:34:53 +0900 Subject: [PATCH 07/11] Modified TactileSensor due to the introduction of BodyCloneMap --- src/Body/TactileSensor.cpp | 3 ++- src/Body/TactileSensor.h | 5 ++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/Body/TactileSensor.cpp b/src/Body/TactileSensor.cpp index ea1c71be3..f16ac0533 100644 --- a/src/Body/TactileSensor.cpp +++ b/src/Body/TactileSensor.cpp @@ -44,7 +44,8 @@ void TactileSensor::copyStateFrom(const DeviceState& other) } -Device* TactileSensor::clone() const +// Device* TactileSensor::clone() const +Device* TactileSensor::doClone(BodyCloneMap*) const { return new TactileSensor(*this, false); } diff --git a/src/Body/TactileSensor.h b/src/Body/TactileSensor.h index 4853e8d32..c44b76f04 100644 --- a/src/Body/TactileSensor.h +++ b/src/Body/TactileSensor.h @@ -22,7 +22,7 @@ class CNOID_EXPORT TactileSensor : public Device virtual const char* typeName() override; void copyStateFrom(const TactileSensor& other); virtual void copyStateFrom(const DeviceState& other) override; - virtual Device* clone() const override; + // virtual Device* clone() const override; virtual DeviceState* cloneState() const override; virtual void forEachActualType(std::function func) override; virtual void clearState() override; @@ -35,6 +35,9 @@ class CNOID_EXPORT TactileSensor : public Device const ForceData& forceData() const { return *forceData_; } ForceData& forceData() { return *forceData_; } + + protected: + virtual Device* doClone(BodyCloneMap* cloneMap) const override; private: From a234075d59d541aefa1c67656230c290301f2d30 Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Fri, 8 Nov 2019 23:09:27 +0900 Subject: [PATCH 08/11] Fixed the TactileSensor problem due to the last updates --- src/Body/TactileSensor.cpp | 3 ++- src/Body/TactileSensor.h | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/Body/TactileSensor.cpp b/src/Body/TactileSensor.cpp index f16ac0533..a0f9b4dbd 100644 --- a/src/Body/TactileSensor.cpp +++ b/src/Body/TactileSensor.cpp @@ -45,7 +45,8 @@ void TactileSensor::copyStateFrom(const DeviceState& other) // Device* TactileSensor::clone() const -Device* TactileSensor::doClone(BodyCloneMap*) const +//Device* TactileSensor::doClone(BodyCloneMap*) const +Referenced* TactileSensor::doClone(CloneMap*) const { return new TactileSensor(*this, false); } diff --git a/src/Body/TactileSensor.h b/src/Body/TactileSensor.h index c44b76f04..57fc3df7a 100644 --- a/src/Body/TactileSensor.h +++ b/src/Body/TactileSensor.h @@ -37,7 +37,8 @@ class CNOID_EXPORT TactileSensor : public Device ForceData& forceData() { return *forceData_; } protected: - virtual Device* doClone(BodyCloneMap* cloneMap) const override; + //virtual Device* doClone(BodyCloneMap* cloneMap) const override; + virtual Referenced* doClone(CloneMap* cloneMap) const override; private: From b2536d79075b249a9e9567318b271840bf98ebc5 Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Wed, 4 Mar 2020 10:39:47 +0900 Subject: [PATCH 09/11] Cleaned the files from my commented out debugging comments --- src/Body/Body.cpp | 2 -- src/Body/ForceSensor.cpp | 4 --- src/Body/ForwardDynamicsABM.cpp | 27 ------------------- src/Body/TactileSensor.cpp | 9 ------- src/Body/TactileSensor.h | 3 --- .../deprecated/VirtualRobotPortHandler.cpp | 3 --- 6 files changed, 48 deletions(-) diff --git a/src/Body/Body.cpp b/src/Body/Body.cpp index fc13b184b..2095aaa63 100644 --- a/src/Body/Body.cpp +++ b/src/Body/Body.cpp @@ -476,8 +476,6 @@ void Body::initializePosition() void Body::initializeState() { - std::cout << "Rafa, in Body::initializeState" << std::endl; - for(auto& link : linkTraverse_){ link->initializeState(); } diff --git a/src/Body/ForceSensor.cpp b/src/Body/ForceSensor.cpp index e60f4081c..cb55bda97 100644 --- a/src/Body/ForceSensor.cpp +++ b/src/Body/ForceSensor.cpp @@ -5,8 +5,6 @@ #include "ForceSensor.h" -#include // Added by Rafa - using namespace cnoid; @@ -96,8 +94,6 @@ const double* ForceSensor::readState(const double* buf) double* ForceSensor::writeState(double* out_buf) const { - // std::cout << "Rafa, in ForceSensor::writeState" << std::endl; - Eigen::Map(out_buf) << F_; return out_buf + 6; } diff --git a/src/Body/ForwardDynamicsABM.cpp b/src/Body/ForwardDynamicsABM.cpp index 68aa21aae..84ff1c09c 100644 --- a/src/Body/ForwardDynamicsABM.cpp +++ b/src/Body/ForwardDynamicsABM.cpp @@ -8,8 +8,6 @@ #include "LinkTraverse.h" #include -#include // Added by Rafa - using namespace std; using namespace cnoid; @@ -506,8 +504,6 @@ void ForwardDynamicsABM::calcABMPhase3() void ForwardDynamicsABM::updateForceSensors() { - // std::cout << "Rafa, in ForwardDynamicsABM::updateForceSensors" << std::endl; - const DeviceList& sensors = sensorHelper.forceSensors(); for(size_t i=0; i < sensors.size(); ++i){ ForceSensor* sensor = sensors[i]; @@ -539,13 +535,9 @@ void ForwardDynamicsABM::updateTactileSensors() DyLink* link = static_cast(sensor->link()); - // std::cout << "Rafa, in ForwardDynamicsABM::updateTactileSensors, for sensor " << i << " link->name() = " << link->name() << std::endl; - DyLink::ConstraintForceArray& forces = link->constraintForces(); if(!forces.empty()){ - // std::cout << "Rafa, in ForwardDynamicsABM::updateTactileSensors, forces are not empty" << std::endl; - sensor->forceData().clear(); for(size_t j=0; j < forces.size(); ++j){ @@ -554,27 +546,8 @@ void ForwardDynamicsABM::updateTactileSensors() Vector3 f_surf = link->R().transpose() * force_surf.force; if (p_surf.z() < EPSILON) { std::pair xy_f = std::make_pair(Vector2(p_surf.x(), p_surf.y()), f_surf); - // Vector2 xy_f = Vector2(p_surf.x(), p_surf.y()); // Rafa, temporal implementation sensor->forceData().push_back(xy_f); sensor->notifyStateChange(); - // Rafa commented this - /* - std::cout << "Rafa, in ForwardDynamicsABM::updateTactileSensors, in link " - << link->name() << " at point (" - << sensor->forceData().back().first.x() << ", " - << sensor->forceData().back().first.y() << ") there is a force (" - << sensor->forceData().back().second.x() << ", " - << sensor->forceData().back().second.y() << ", " - << sensor->forceData().back().second.z() << ")" - << std::endl; - */ - /* - std::cout << "Rafa, in ForwardDynamicsABM::updateTactileSensors, in link " - << link->name() << " at point (" - << sensor->forceData().back().x() << ", " - << sensor->forceData().back().y() << ") there is a force" - << std::endl; // Rafa, temporal implementation - */ } } } diff --git a/src/Body/TactileSensor.cpp b/src/Body/TactileSensor.cpp index a0f9b4dbd..16e43c382 100644 --- a/src/Body/TactileSensor.cpp +++ b/src/Body/TactileSensor.cpp @@ -5,8 +5,6 @@ #include "TactileSensor.h" -#include // Rafa, this is temporal - using namespace cnoid; @@ -43,9 +41,6 @@ void TactileSensor::copyStateFrom(const DeviceState& other) copyStateFrom(static_cast(other)); } - -// Device* TactileSensor::clone() const -//Device* TactileSensor::doClone(BodyCloneMap*) const Referenced* TactileSensor::doClone(CloneMap*) const { return new TactileSensor(*this, false); @@ -73,10 +68,7 @@ void TactileSensor::clearState() int TactileSensor::stateSize() const { - // std::cout << "Rafa, in TactileSensor::stateSize, forceData_->size() = " << forceData_->size() << std::endl; - return forceData_->size() * 5; - // return forceData_->size() * 2; // Rafa, temporal implementation } @@ -90,7 +82,6 @@ double* TactileSensor::writeState(double* out_buf) const { for (size_t i = 0; i < forceData_->size(); i++) { Eigen::Map(out_buf) << (*forceData_)[i].first; - //Eigen::Map(out_buf) << (*forceData_)[i]; // Rafa, temporal implementation out_buf = out_buf + 2; Eigen::Map(out_buf) << (*forceData_)[i].second; out_buf = out_buf + 3; diff --git a/src/Body/TactileSensor.h b/src/Body/TactileSensor.h index 57fc3df7a..d6c3b596d 100644 --- a/src/Body/TactileSensor.h +++ b/src/Body/TactileSensor.h @@ -22,7 +22,6 @@ class CNOID_EXPORT TactileSensor : public Device virtual const char* typeName() override; void copyStateFrom(const TactileSensor& other); virtual void copyStateFrom(const DeviceState& other) override; - // virtual Device* clone() const override; virtual DeviceState* cloneState() const override; virtual void forEachActualType(std::function func) override; virtual void clearState() override; @@ -31,13 +30,11 @@ class CNOID_EXPORT TactileSensor : public Device virtual double* writeState(double* out_buf) const override; typedef std::vector< std::pair > ForceData; - // typedef std::vector ForceData; // Rafa, temporal implementation const ForceData& forceData() const { return *forceData_; } ForceData& forceData() { return *forceData_; } protected: - //virtual Device* doClone(BodyCloneMap* cloneMap) const override; virtual Referenced* doClone(CloneMap* cloneMap) const override; private: diff --git a/src/OpenRTMPlugin/deprecated/VirtualRobotPortHandler.cpp b/src/OpenRTMPlugin/deprecated/VirtualRobotPortHandler.cpp index c887b1875..3522cb629 100644 --- a/src/OpenRTMPlugin/deprecated/VirtualRobotPortHandler.cpp +++ b/src/OpenRTMPlugin/deprecated/VirtualRobotPortHandler.cpp @@ -312,7 +312,6 @@ SensorDataOutPortHandler::SensorDataOutPortHandler(PortInfo& info) outPort(info.portName.c_str(), value), sensorNames(info.dataOwnerNames) { - // std::cout << "Rafa, in SensorDataOutPortHandler::SensorDataOutPortHandler, sensorNames.size() = " << sensorNames.size() << std::endl; } @@ -327,13 +326,11 @@ void SensorDataOutPortHandler::inputDataFromSimulator(BodyRTCItem* bodyRTC) auto body = bodyRTC->body(); if(!sensorNames.empty()){ if(Device* sensor = body->findDevice(sensorNames[0])){ - // std::cout << "Rafa, in SensorDataOutPortHandler::inputDataFromSimulator, first check, sensor->typeName = " << sensor->typeName() << std::endl; const int dataSize = sensor->stateSize(); value.data.length(dataSize); if(dataSize > 0){ for(size_t i=0; i < sensorNames.size(); ++i){ if(Device* sensor = body->findDevice(sensorNames[i])){ - // std::cout << "Rafa, in SensorDataOutPortHandler::inputDataFromSimulator, if dataSize > 0, sensor->typeName = " << sensor->typeName() << std::endl; sensor->writeState(&value.data[i * dataSize]); } } From e9ca5f5654a172da6930150076ab438524c2ccb9 Mon Sep 17 00:00:00 2001 From: Nse Obot Date: Tue, 17 Mar 2020 15:26:43 +0900 Subject: [PATCH 10/11] generalize tactile sensor --- src/Body/ForwardDynamicsABM.cpp | 71 +++++++++++++++++++++++++-------- src/Body/TactileSensor.cpp | 23 ++++++++++- src/Body/TactileSensor.h | 22 +++++++++- src/Body/VRMLBodyLoader.cpp | 6 +++ 4 files changed, 103 insertions(+), 19 deletions(-) diff --git a/src/Body/ForwardDynamicsABM.cpp b/src/Body/ForwardDynamicsABM.cpp index 84ff1c09c..00f77066f 100644 --- a/src/Body/ForwardDynamicsABM.cpp +++ b/src/Body/ForwardDynamicsABM.cpp @@ -533,23 +533,60 @@ void ForwardDynamicsABM::updateTactileSensors() TactileSensor* sensor = sensors[i]; - DyLink* link = static_cast(sensor->link()); - - DyLink::ConstraintForceArray& forces = link->constraintForces(); - if(!forces.empty()){ - - sensor->forceData().clear(); - - for(size_t j=0; j < forces.size(); ++j){ - const DyLink::ConstraintForce& force_surf = forces[j]; - Vector3 p_surf = link->R().transpose() * (force_surf.point - link->p()); - Vector3 f_surf = link->R().transpose() * force_surf.force; - if (p_surf.z() < EPSILON) { - std::pair xy_f = std::make_pair(Vector2(p_surf.x(), p_surf.y()), f_surf); - sensor->forceData().push_back(xy_f); - sensor->notifyStateChange(); + DyLink* link = static_cast(sensor->link()); + + DyLink::ConstraintForceArray& forces = link->constraintForces(); + if(!forces.empty()){ + + sensor->forceData().clear(); + vector> points; + double xRange = sensor->maxX() - sensor->minX(); + double yRange = sensor->maxY() - sensor->minY(); + double xCellSize = xRange/sensor->cols(); + double yCellSize = yRange/sensor->rows(); + double currentX = sensor->minX(); + double currentY = sensor->minY(); + + for (int j = 0; jrows()*sensor->cols(); j++){ + points.push_back(make_tuple(currentX, currentY, 0)); + currentX += xCellSize; + if ((j+1)%sensor->cols() == 0){ + currentX = sensor->minX(); + currentY += yCellSize; + } + } + + for(size_t j=0; j < forces.size(); ++j){ + const DyLink::ConstraintForce& force_surf = forces[j]; + Vector3 p_surf = link->R().transpose() * (force_surf.point - link->p()); + Vector3 f_surf = link->R().transpose() * force_surf.force; + if (p_surf.z() < sensor->p_local().z() + EPSILON) { + double x = p_surf.x(); + double y = p_surf.y(); + + //saturate + if (x>sensor->maxX()) x = sensor->maxX(); + if (y>sensor->maxY()) y = sensor->maxY(); + if (xminX()) x = sensor->minX(); + if (yminY()) y = sensor->minY(); + + double absX = x - sensor->minX(); + double absY = y - sensor->minY(); + int matrixX = floor(absX/xCellSize); + int matrixY = floor(absY/yCellSize); + if (matrixX >= sensor->cols()) matrixX = sensor->cols()-1; + if (matrixY >= sensor->rows()) matrixY = sensor->rows()-1; + + get<2>(points.at(matrixY*sensor->cols() + matrixX)) = get<2>(points.at(matrixY*sensor->cols() + matrixX)) + f_surf.z(); + } + } + for (int j=0; j(points.at(j)) > 0){ + std::pair xy_f = std::make_pair( Vector2(get<0>(points.at(j)), get<1>(points.at(j))) , Vector3(0,0, get<2>(points.at(j))) ); + sensor->forceData().push_back(xy_f); + sensor->notifyStateChange(); + } + } } - } - } } } diff --git a/src/Body/TactileSensor.cpp b/src/Body/TactileSensor.cpp index 16e43c382..b120e5948 100644 --- a/src/Body/TactileSensor.cpp +++ b/src/Body/TactileSensor.cpp @@ -8,9 +8,10 @@ using namespace cnoid; -TactileSensor::TactileSensor() +TactileSensor::TactileSensor() //: maxX_(0), maxY_(0), minX_(0), minY_(0), rows_(0), cols_(0) { forceData_ = std::make_shared(); + maxX_ = maxY_ = minX_ = minY_ = rows_ = cols_ = 0; } @@ -30,6 +31,12 @@ const char* TactileSensor::typeName() void TactileSensor::copyStateFrom(const TactileSensor& other) { forceData_ = other.forceData_; + maxX_ = other.maxX_; + maxY_ = other.maxY_; + minX_ = other.minX_; + minY_ = other.minY_; + rows_ = other.rows_; + cols_ = other.cols_; } @@ -89,3 +96,17 @@ double* TactileSensor::writeState(double* out_buf) const return out_buf; } + +void TactileSensor::setMaxX(double maxX) {maxX_ = maxX;} +void TactileSensor::setMaxY(double maxY) {maxY_ = maxY;} +void TactileSensor::setMinX(double minX) {minX_ = minX;} +void TactileSensor::setMinY(double minY) {minY_ = minY;} +void TactileSensor::setRows(int rows) {rows_ = rows;} +void TactileSensor::setCols(int cols) {cols_ = cols;} + +double TactileSensor::maxX() {return maxX_;} +double TactileSensor::maxY() {return maxY_;} +double TactileSensor::minY() {return minY_;} +double TactileSensor::minX() {return minX_;} +int TactileSensor::rows() {return rows_;} +int TactileSensor::cols() {return cols_;} diff --git a/src/Body/TactileSensor.h b/src/Body/TactileSensor.h index d6c3b596d..89083dd15 100644 --- a/src/Body/TactileSensor.h +++ b/src/Body/TactileSensor.h @@ -9,7 +9,7 @@ #include "Device.h" #include #include "exportdecl.h" - +#include namespace cnoid { class CNOID_EXPORT TactileSensor : public Device @@ -33,6 +33,20 @@ class CNOID_EXPORT TactileSensor : public Device const ForceData& forceData() const { return *forceData_; } ForceData& forceData() { return *forceData_; } + + void setMaxX(double maxX); + void setMaxY(double maxY); + void setMinX(double minX); + void setMinY(double minY); + void setRows(int rows); + void setCols(int cols); + + double maxX(); + double maxY(); + double minY(); + double minX(); + int rows(); + int cols(); protected: virtual Referenced* doClone(CloneMap* cloneMap) const override; @@ -40,6 +54,12 @@ class CNOID_EXPORT TactileSensor : public Device private: std::shared_ptr forceData_; + double maxX_; + double maxY_; + double minX_; + double minY_; + int rows_; + int cols_; }; typedef ref_ptr TactileSensorPtr; diff --git a/src/Body/VRMLBodyLoader.cpp b/src/Body/VRMLBodyLoader.cpp index 069422d21..cc85cb70b 100644 --- a/src/Body/VRMLBodyLoader.cpp +++ b/src/Body/VRMLBodyLoader.cpp @@ -1196,6 +1196,12 @@ TactileSensorPtr VRMLBodyLoaderImpl::createTactileSensor(VRMLProtoInstance* node TactileSensorPtr sensor = new TactileSensor(); readDeviceCommonParameters(*sensor, node); + sensor->setMaxX(getValue(node, "maxX")); + sensor->setMaxY(getValue(node, "maxY")); + sensor->setMinX(getValue(node, "minX")); + sensor->setMinY(getValue(node, "minY")); + sensor->setRows(getValue(node, "rows")); + sensor->setCols(getValue(node, "cols")); return sensor; } From 4733c688ea9eb3c65869b96f78a4ba07a625d46c Mon Sep 17 00:00:00 2001 From: Nse Obot Date: Thu, 19 Mar 2020 11:19:35 +0900 Subject: [PATCH 11/11] allow zero force --- src/Body/ForwardDynamicsABM.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/Body/ForwardDynamicsABM.cpp b/src/Body/ForwardDynamicsABM.cpp index 00f77066f..4e07d617d 100644 --- a/src/Body/ForwardDynamicsABM.cpp +++ b/src/Body/ForwardDynamicsABM.cpp @@ -548,14 +548,13 @@ void ForwardDynamicsABM::updateTactileSensors() double currentY = sensor->minY(); for (int j = 0; jrows()*sensor->cols(); j++){ - points.push_back(make_tuple(currentX, currentY, 0)); + points.push_back(make_tuple(currentX, currentY, -EPSILON)); currentX += xCellSize; if ((j+1)%sensor->cols() == 0){ currentX = sensor->minX(); currentY += yCellSize; } } - for(size_t j=0; j < forces.size(); ++j){ const DyLink::ConstraintForce& force_surf = forces[j]; Vector3 p_surf = link->R().transpose() * (force_surf.point - link->p()); @@ -563,13 +562,11 @@ void ForwardDynamicsABM::updateTactileSensors() if (p_surf.z() < sensor->p_local().z() + EPSILON) { double x = p_surf.x(); double y = p_surf.y(); - //saturate if (x>sensor->maxX()) x = sensor->maxX(); if (y>sensor->maxY()) y = sensor->maxY(); if (xminX()) x = sensor->minX(); if (yminY()) y = sensor->minY(); - double absX = x - sensor->minX(); double absY = y - sensor->minY(); int matrixX = floor(absX/xCellSize); @@ -577,11 +574,16 @@ void ForwardDynamicsABM::updateTactileSensors() if (matrixX >= sensor->cols()) matrixX = sensor->cols()-1; if (matrixY >= sensor->rows()) matrixY = sensor->rows()-1; - get<2>(points.at(matrixY*sensor->cols() + matrixX)) = get<2>(points.at(matrixY*sensor->cols() + matrixX)) + f_surf.z(); + if (get<2>(points.at(matrixY*sensor->cols() + matrixX)) == -EPSILON){ + get<2>(points.at(matrixY*sensor->cols() + matrixX)) = f_surf.z(); + } + else{ + get<2>(points.at(matrixY*sensor->cols() + matrixX)) = get<2>(points.at(matrixY*sensor->cols() + matrixX)) + f_surf.z(); + } } } for (int j=0; j(points.at(j)) > 0){ + if (get<2>(points.at(j)) > -EPSILON){ std::pair xy_f = std::make_pair( Vector2(get<0>(points.at(j)), get<1>(points.at(j))) , Vector3(0,0, get<2>(points.at(j))) ); sensor->forceData().push_back(xy_f); sensor->notifyStateChange();