diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp index 13891cfb6f89acd3c26192c4ad13f3374cbdec2d..a277ffe9033392861dd6acdc8a20cb32aeb73f37 100644 --- a/source/RobotAPI/libraries/core/Trajectory.cpp +++ b/source/RobotAPI/libraries/core/Trajectory.cpp @@ -1827,7 +1827,26 @@ namespace armarx } + void Trajectory::scaleValue(const Ice::DoubleSeq& factor) + { + if (factor.size() > dim()) + { + throw LocalException("dimension is out of range: ") << factor.size() << " actual dimensions: " << dim(); + } + + for (size_t dimension = 0; dimension < dim(); dimension++) + { + const ordered_view& ordv = dataMap.get<TagOrdered>(); + typename ordered_view::const_iterator itMap = ordv.begin(); + + for (; itMap != ordv.end(); itMap++) + { + itMap->data[dimension]->at(0) *= factor[dimension]; + } + } + + } } // namespace armarx diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h index 33422bc0098b83a4fb23ffee3bfc099d77f46d58..db38b1e6657b9717dd2f0f98bd418b9d5a02e234 100644 --- a/source/RobotAPI/libraries/core/Trajectory.h +++ b/source/RobotAPI/libraries/core/Trajectory.h @@ -390,6 +390,7 @@ namespace armarx void shiftTime(double shift); void shiftValue(const Ice::DoubleSeq& shift); + void scaleValue(const Ice::DoubleSeq& factor); static void CopyData(const Trajectory& source, Trajectory& destination);