45 if (other.size() != data.rows())
ThrowPretty(
"Wrong initializer size: " << other.size() <<
" expecting " << data.rows());
47 for (
const double& val : other)
56 data = Eigen::VectorXd::Zero(n);
66 if (data.rows() != other.
data.rows())
ThrowPretty(
"Task space vector sizes do not match!");
69 Eigen::VectorXd ret(data.rows() + map.size() * 3 - entry_size);
74 if (i_in < entry.id) ret.segment(i_out, entry.id - i_in) = data.segment(i_in, entry.id - i_in) - other.
data.segment(i_in, entry.id - i_in);
75 i_out += entry.id - i_in;
76 i_in += entry.id - i_in;
83 ret(i_out) = rotvec[0];
84 ret(i_out + 1) = rotvec[1];
85 ret(i_out + 2) = rotvec[2];
89 if (i_in < data.rows()) ret.segment(i_out, data.rows() - i_in) = data.segment(i_in, data.rows() - i_in) - other.
data.segment(i_in, data.rows() - i_in);
93 std::vector<TaskVectorEntry>
TaskVectorEntry::reindex(
const std::vector<TaskVectorEntry>& _map,
int _old_start,
int _new_start)
95 std::vector<TaskVectorEntry> ret = _map;
98 entry.id = entry.id - _old_start + _new_start;
void SetZero(const int n)
TaskSpaceVector & operator=(std::initializer_list< double > other)
KDL::Rotation GetRotation(Eigen::VectorXdRefConst data, RotationType type)
static std::vector< TaskVectorEntry > reindex(const std::vector< TaskVectorEntry > &_map, int _old_start, int _new_start)
Eigen::VectorXd operator-(const TaskSpaceVector &other)
Eigen::VectorXd SetRotation(const KDL::Rotation &data, RotationType type)
int GetRotationTypeLength(const RotationType &type)