Pull request #780 » 0001-fix-RobotState-and-remove-useless-functions.patch
| src/API/ConfigSpace/RobotState.cpp | ||
|---|---|---|
|
}
|
||
|
configPt RobotState::getConfigStruct() {
|
||
|
return &state[0][0];
|
||
|
if(state.size() && state[0].size())
|
||
|
return &state[0][0];
|
||
|
else
|
||
|
return NULL;
|
||
|
}
|
||
|
vector<vector<double> > RobotState::getState() const {
|
||
| ... | ... | |
|
bool RobotState::operator!=(RobotState &Conf) { return !(this->equal (Conf)); }
|
||
|
statePtr_t RobotState::operator-(RobotState &Conf) { return this->sub (Conf); }
|
||
|
//statePtr_t RobotState::operator-(RobotState &Conf) { return this->sub (Conf); }
|
||
|
statePtr_t RobotState::operator+(RobotState &Conf) { return this->add (Conf); }
|
||
|
//statePtr_t RobotState::operator+(RobotState &Conf) { return this->add (Conf); }
|
||
|
RobotState &RobotState::operator*(const double coeff) { return this->mult (coeff); }
|
||
|
//RobotState &RobotState::operator*(const double coeff) { return this->mult (coeff); }
|
||
|
double &RobotState::at(const int &i) { return getConfigStruct ()[i]; }
|
||
|
statePtr_t RobotState::add(RobotState &C) {
|
||
|
shared_ptr<RobotState> ptrQ (new RobotState (_Robot));
|
||
|
p3d_addConfig (_Robot->getRobotStruct (), getConfigStruct (),
|
||
|
C.getConfigStruct (), ptrQ->getConfigStruct ());
|
||
|
return ptrQ;
|
||
|
}
|
||
|
statePtr_t RobotState::sub(RobotState &C) {
|
||
|
statePtr_t ptrQ (new RobotState (_Robot));
|
||
|
p3d_subConfig (_Robot->getRobotStruct (), getConfigStruct (),
|
||
|
C.getConfigStruct (), ptrQ->getConfigStruct ());
|
||
|
return ptrQ;
|
||
|
}
|
||
|
RobotState &RobotState::mult(double coeff) {
|
||
|
RobotState *q = new RobotState (*this);
|
||
|
//statePtr_t RobotState::add(RobotState &C) {
|
||
|
// shared_ptr<RobotState> ptrQ (new RobotState (_Robot));
|
||
|
//
|
||
|
// p3d_addConfig (_Robot->getRobotStruct (), getConfigStruct (),
|
||
|
// C.getConfigStruct (), ptrQ->getConfigStruct ());
|
||
|
//
|
||
|
// return ptrQ;
|
||
|
//}
|
||
|
//
|
||
|
//statePtr_t RobotState::sub(RobotState &C) {
|
||
|
// statePtr_t ptrQ (new RobotState (_Robot));
|
||
|
//
|
||
|
// p3d_subConfig (_Robot->getRobotStruct (), getConfigStruct (),
|
||
|
// C.getConfigStruct (), ptrQ->getConfigStruct ());
|
||
|
//
|
||
|
// return ptrQ;
|
||
|
//}
|
||
|
int njnt = _Robot->getRobotStruct ()->njoints;
|
||
|
int k = 0;
|
||
|
for (int i = 0; i <= njnt; i++) {
|
||
|
p3d_jnt *jntPt = _Robot->getRobotStruct ()->joints[i];
|
||
|
for (int j = 0; j < jntPt->dof_equiv_nbr; j++) {
|
||
|
(*q)[k] *= coeff;
|
||
|
k++;
|
||
|
}
|
||
|
}
|
||
|
return (*q);
|
||
|
}
|
||
|
// RobotState &RobotState::mult(double coeff) {
|
||
|
// RobotState *q = new RobotState (*this);
|
||
|
//
|
||
|
// int njnt = _Robot->getRobotStruct ()->njoints;
|
||
|
// int k = 0;
|
||
|
// for (int i = 0; i <= njnt; i++) {
|
||
|
// p3d_jnt *jntPt = _Robot->getRobotStruct ()->joints[i];
|
||
|
// for (int j = 0; j < jntPt->dof_equiv_nbr; j++) {
|
||
|
// (*q)[k] *= coeff;
|
||
|
// k++;
|
||
|
// }
|
||
|
// }
|
||
|
//
|
||
|
// return (*q);
|
||
|
// }
|
||
|
Eigen::VectorXd RobotState::getEigenVector() {
|
||
|
unsigned int nbDof = 0;
|
||
| src/API/ConfigSpace/RobotState.hpp | ||
|---|---|---|
|
bool operator!=(RobotState &Conf);
|
||
|
statePtr_t operator-(RobotState &Conf);
|
||
|
//needs to be updated to take into account full state
|
||
|
//statePtr_t operator-(RobotState &Conf);
|
||
|
statePtr_t operator+(RobotState &Conf);
|
||
|
//needs to be updated to take into account full state
|
||
|
//statePtr_t operator+(RobotState &Conf);
|
||
|
RobotState &operator*(const double coeff);
|
||
|
//needs to be updated to take into account full state
|
||
|
//RobotState &operator*(const double coeff);
|
||
|
RobotState &operator=(const RobotState &other);
|
||
|
statePtr_t sub(RobotState &C);
|
||
|
//needs to be updated to take into account full state
|
||
|
//statePtr_t sub(RobotState &C);
|
||
|
statePtr_t add(RobotState &C);
|
||
|
//needs to be updated to take into account full state
|
||
|
//statePtr_t add(RobotState &C);
|
||
|
RobotState &mult(double coeff);
|
||
|
//needs to be updated to take into account full state
|
||
|
//RobotState &mult(double coeff);
|
||
|
bool equal(RobotState &Conf, bool print = false);
|
||
| src/HRI_costspace/CostFunctions/HRICS_CF_TimeToCollision.cpp | ||
|---|---|---|
|
//cprev was dt seconds before ccurrent. Returned configuration is the projected one at time t seconds after ccurrent.
|
||
|
Robot* rob = cprev->getRobot();
|
||
|
statePtr_t cnext(new RobotState(rob)),cdiff(new RobotState(rob));
|
||
|
cdiff = *cprev - *ccurrent;
|
||
|
//Robot* rob = cprev->getRobot();
|
||
|
//statePtr_t cnext(new RobotState(rob)),cdiff(new RobotState(rob));
|
||
|
//cdiff = *cprev - *ccurrent;
|
||
|
cnext = *ccurrent + (*cdiff * (t/dt));
|
||
|
return cnext;
|
||
|
//cnext = *ccurrent + (*cdiff * (t/dt));
|
||
|
//return cnext;
|
||
|
}
|
||
|
int TimeToCollision::test(CostFunctionCommon* common){
|
||