1 #include "trajopt/configuration_space.hpp"
6 OpenRAVE::KinBodyPtr m_body;
10 virtual void SetDOFValues(
const DblVec& dofs) {}
11 virtual DblVec GetDOFValues() {
return DblVec();};
12 virtual int GetDOF()
const {
return 0;}
13 virtual void GetDOFLimits(DblVec&, DblVec&)
const {}
14 virtual OpenRAVE::EnvironmentBasePtr GetEnv() {
15 return m_body->GetEnv();
17 virtual DblMatrix PositionJacobian(
int link_ind,
const OR::Vector& pt)
const {
18 return MatrixXd::Zero(3,0);
20 virtual DblMatrix RotationJacobian(
int link_ind)
const {
21 PRINT_AND_THROW(
"not implemented");
23 virtual bool DoesAffect(
const KinBody::Link& link) {
24 BOOST_FOREACH(
const KinBody::LinkPtr& link1, m_body->GetLinks()) {
25 if (link1.get() == &link)
30 virtual std::vector<KinBody::LinkPtr> GetAffectedLinks() {
31 return m_body->GetLinks();
33 virtual void GetAffectedLinks(std::vector<OpenRAVE::KinBody::LinkPtr>& links,
34 bool only_with_geom, vector<int>& link_inds) {
35 links = GetAffectedLinks();
36 link_inds.resize(links.size());
37 for (
int i = 0; i < links.size(); ++i)
38 link_inds.push_back(links[i]->GetIndex());
40 virtual DblVec RandomDOFValues() {
41 return toDblVec(VectorXd::Random(0));
43 virtual vector<OpenRAVE::KinBodyPtr> GetBodies() {
return vector<OpenRAVE::KinBodyPtr>(1, m_body);}
45 typedef boost::shared_ptr<StaticObject> StaticObjectPtr;