46 #include <boost/lexical_cast.hpp> 59 if ( bag.
getType() ==
"KDL.Joint" ){
61 if ( !type_name.
ready() )
63 if (type_name.
value()==
"RotX"){
66 }
else if(type_name.
value()==
"RotY"){
69 }
else if(type_name.
value()==
"RotZ"){
72 }
else if(type_name.
value()==
"TransX"){
75 }
else if(type_name.
value()==
"TransY"){
78 }
else if(type_name.
value()==
"TransZ"){
81 }
else if(type_name.
value()==
"None"){
95 targetbag.
setType(
"KDL.Segment");
99 targetbag.
add(
new Property<PropertyBag>(
"frame",
"The offset frame from the joint at the base to the end of the link",frame_bag));
104 if ( bag.
getType() ==
"KDL.Segment" ){
107 if(!(frame_bag.ready()&&joint_bag.
ready()))
111 joint_prop.getDataSource());
112 Property<Frame> frame_prop(frame_bag.getName(),frame_bag.getDescription());
114 frame_prop.getDataSource());
115 segment=
Segment(joint_prop.value(),frame_prop.value());
124 targetbag.
setType(
"KDL.Chain");
135 if( bag.
getType() ==
"KDL.Chain"){
136 for(
unsigned int i=0;i<bag.
size();i++){
138 if(!segment_bag->
ready())
143 segment_prop.getDataSource());
154 targetbag.
setType(
"KDL.JntArray");
155 for(
int i = 0; i < jntarray.
data.rows(); i++)
157 std::string rindx = boost::lexical_cast<std::string>( i );
165 if( bag.
getType() ==
"KDL.JntArray"){
166 for(
unsigned int i = 0; i < bag.
size(); i++){
168 if(!jntarray_bag->
ready())
172 jntarray_new.data(i) = jntarray_prop.value();
174 jntarray = jntarray_new;
182 targetbag.
setType(
"KDL.Jacobian");
183 for(
int i = 0; i < jacobian.
data.rows(); i++){
184 for(
int j = 0; j < jacobian.
data.cols(); j++){
185 std::string rindx = boost::lexical_cast<std::string>( i );
186 std::string cindx = boost::lexical_cast<std::string>( j );
187 targetbag.
add(
new Property<double>(
"Element (" + rindx +
"," + cindx +
")",
"Jacobian element",jacobian.
data(i,j)));
196 if( bag.
getType() ==
"KDL.Jacobian"){
197 for(
unsigned int i = 0; i < 6; i++){
198 for(
unsigned int j = 0; j < bag.
size() / 6; j++){
200 if(!data_bag->
ready())
204 jacobian_new.
data(i,j) = data_prop.value();
207 jacobian = jacobian_new;
virtual base::DataSourceBase::shared_ptr getDataSource() const
bool composeType(base::DataSourceBase::shared_ptr source, base::DataSourceBase::shared_ptr target) const
void add(base::PropertyBase *p)
const Segment & getSegment(unsigned int nr) const
Property< T > * getPropertyType(const std::string &name) const
const std::string & getType() const
bool composeProperty(const PropertyBag &bag, Joint &joint)
unsigned int getNrOfSegments() const
void setType(const std::string &newtype)
void addSegment(const Segment &segment)
Frame getFrameToTip() const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 6, Eigen::Dynamic > data
void decomposeProperty(const Joint &joint, PropertyBag &targetbag)
const Joint & getJoint() const
const std::string & getDescription() const
virtual const types::TypeInfo * getTypeInfo() const
base::PropertyBase * getItem(int i) const
const std::string & getName() const
const std::string getTypeName() const
virtual DataSourceBase::shared_ptr getDataSource() const =0