Obbart/RoboGlue_ROS
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Modifica angoli RPY per tenere utensile perpendicolare al terreno

Browse Source
This commit is contained in:
Emanuele
2019-11-19 09:20:45 +01:00
parent dde6d1e083
commit ba5ed10349
3 changed files with 55 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

100
src/roboglue_ros/config/data/test99.data
View File

@@ -1,51 +1,51 @@
N,X,Y,Z,rR,rP,rY,DT,DS N,X,Y,Z,rR,rP,rY,DT,DS
N0,0.018000,0.306000,0.500000,1.570000,-2.680000,0.000000,0.000000,0.000000 N0,0.018000,0.306000,0.500000,0.000000,1.571000,1.571000,0.000000,0.000000
N1,0.018000,0.312000,0.500000,1.570000,-2.680000,0.000000,0.095493,0.006000 N1,0.018000,0.312000,0.500000,0.000000,1.571000,1.571000,0.095493,0.006000
N2,0.018000,0.318000,0.500000,1.570000,-2.680000,0.000000,0.177313,0.006000 N2,0.018000,0.318000,0.500000,0.000000,1.571000,1.571000,0.177313,0.006000
N3,0.018000,0.324000,0.500000,1.570000,-2.680000,0.000000,0.276251,0.006000 N3,0.018000,0.324000,0.500000,0.000000,1.571000,1.571000,0.276251,0.006000
N4,-0.006000,0.324000,0.500000,1.570000,-2.680000,0.000000,0.376993,0.024000 N4,-0.006000,0.324000,0.500000,0.000000,1.571000,1.571000,0.376993,0.024000
N5,-0.024000,0.312000,0.500000,1.570000,-2.680000,0.000000,0.477276,0.021633 N5,-0.024000,0.312000,0.500000,0.000000,1.571000,1.571000,0.477276,0.021633
N6,-0.030000,0.288000,0.500000,1.570000,-2.680000,0.000000,0.583836,0.024739 N6,-0.030000,0.288000,0.500000,0.000000,1.571000,1.571000,0.583836,0.024739
N7,-0.048000,0.258000,0.500000,1.570000,-2.680000,0.000000,0.674398,0.034986 N7,-0.048000,0.258000,0.500000,0.000000,1.571000,1.571000,0.674398,0.034986
N8,-0.060000,0.246000,0.500000,1.570000,-2.680000,0.000000,0.779036,0.016971 N8,-0.060000,0.246000,0.500000,0.000000,1.571000,1.571000,0.779036,0.016971
N9,-0.066000,0.240000,0.500000,1.570000,-2.680000,0.000000,0.878682,0.008485 N9,-0.066000,0.240000,0.500000,0.000000,1.571000,1.571000,0.878682,0.008485
N10,-0.078000,0.240000,0.500000,1.570000,-2.680000,0.000000,0.985584,0.012000 N10,-0.078000,0.240000,0.500000,0.000000,1.571000,1.571000,0.985584,0.012000
N11,-0.126000,0.240000,0.500000,1.570000,-2.680000,0.000000,1.076938,0.048000 N11,-0.126000,0.240000,0.500000,0.000000,1.571000,1.571000,1.076938,0.048000
N12,-0.150000,0.240000,0.500000,1.570000,-2.680000,0.000000,1.173122,0.024000 N12,-0.150000,0.240000,0.500000,0.000000,1.571000,1.571000,1.173122,0.024000
N13,-0.162000,0.252000,0.500000,1.570000,-2.680000,0.000000,1.278250,0.016971 N13,-0.162000,0.252000,0.500000,0.000000,1.571000,1.571000,1.278250,0.016971
N14,-0.174000,0.258000,0.500000,1.570000,-2.680000,0.000000,1.378685,0.013416 N14,-0.174000,0.258000,0.500000,0.000000,1.571000,1.571000,1.378685,0.013416
N15,-0.186000,0.276000,0.500000,1.570000,-2.680000,0.000000,1.484213,0.021633 N15,-0.186000,0.276000,0.500000,0.000000,1.571000,1.571000,1.484213,0.021633
N16,-0.198000,0.300000,0.500000,1.570000,-2.680000,0.000000,1.576499,0.026833 N16,-0.198000,0.300000,0.500000,0.000000,1.571000,1.571000,0.006499,0.026833
N17,-0.204000,0.324000,0.500000,1.570000,-2.680000,0.000000,1.690293,0.024739 N17,-0.204000,0.324000,0.500000,0.000000,1.571000,1.571000,1.690293,0.024739
N18,-0.204000,0.342000,0.500000,1.570000,-2.680000,0.000000,1.780065,0.018000 N18,-0.204000,0.342000,0.500000,0.000000,1.571000,1.571000,1.780065,0.018000
N19,-0.204000,0.378000,0.500000,1.570000,-2.680000,0.000000,1.872321,0.036000 N19,-0.204000,0.378000,0.500000,0.000000,1.571000,1.571000,1.872321,0.036000
N20,-0.222000,0.450000,0.500000,1.570000,-2.680000,0.000000,1.977848,0.074216 N20,-0.222000,0.450000,0.500000,0.000000,1.571000,1.571000,1.977848,0.074216
N21,-0.318000,0.468000,0.500000,1.570000,-2.680000,0.000000,2.070969,0.097673 N21,-0.318000,0.468000,0.500000,0.000000,1.571000,1.571000,2.070969,0.097673
N22,-0.396000,0.450000,0.500000,1.570000,-2.680000,0.000000,2.177687,0.080050 N22,-0.396000,0.450000,0.500000,0.000000,1.571000,1.571000,2.177687,0.080050
N23,-0.462000,0.348000,0.500000,1.570000,-2.680000,0.000000,2.283137,0.121491 N23,-0.462000,0.348000,0.500000,0.000000,1.571000,1.571000,2.283137,0.121491
N24,-0.558000,0.264000,0.500000,1.570000,-2.680000,0.000000,2.392030,0.127562 N24,-0.558000,0.264000,0.500000,0.000000,1.571000,1.571000,2.392030,0.127562
N25,-0.636000,0.246000,0.500000,1.570000,-2.680000,0.000000,2.479188,0.080050 N25,-0.636000,0.246000,0.500000,0.000000,1.571000,1.571000,2.479188,0.080050
N26,-0.702000,0.306000,0.500000,1.570000,-2.680000,0.000000,2.581360,0.089196 N26,-0.702000,0.306000,0.500000,0.000000,1.571000,1.571000,2.581360,0.089196
N27,-0.702000,0.402000,0.500000,1.570000,-2.680000,0.000000,2.672705,0.096000 N27,-0.702000,0.402000,0.500000,0.000000,1.571000,1.571000,2.672705,0.096000
N28,-0.618000,0.480000,0.500000,1.570000,-2.680000,0.000000,2.780175,0.114630 N28,-0.618000,0.480000,0.500000,0.000000,1.571000,1.571000,2.780175,0.114630
N29,-0.540000,0.468000,0.500000,1.570000,-2.680000,0.000000,2.887023,0.078918 N29,-0.540000,0.468000,0.500000,0.000000,1.571000,1.571000,2.887023,0.078918
N30,-0.300000,0.384000,0.500000,1.570000,-2.680000,0.000000,2.980252,0.254275 N30,-0.300000,0.384000,0.500000,0.000000,1.571000,1.571000,2.980252,0.254275
N31,-0.126000,0.336000,0.500000,1.570000,-2.680000,0.000000,3.088443,0.180499 N31,-0.126000,0.336000,0.500000,0.000000,1.571000,1.571000,3.088443,0.180499
N32,-0.012000,0.318000,0.500000,1.570000,-2.680000,0.000000,3.172616,0.115412 N32,-0.012000,0.318000,0.500000,0.000000,1.571000,1.571000,3.172616,0.115412
N33,0.042000,0.372000,0.500000,1.570000,-2.680000,0.000000,3.277146,0.076368 N33,0.042000,0.372000,0.500000,0.000000,1.571000,1.571000,3.277146,0.076368
N34,0.042000,0.396000,0.500000,1.570000,-2.680000,0.000000,3.385895,0.024000 N34,0.042000,0.396000,0.500000,0.000000,1.571000,1.571000,3.385895,0.024000
N35,0.024000,0.390000,0.500000,1.570000,-2.680000,0.000000,3.478245,0.018974 N35,0.024000,0.390000,0.500000,0.000000,1.571000,1.571000,3.478245,0.018974
N36,0.000000,0.288000,0.500000,1.570000,-2.680000,0.000000,3.585851,0.104785 N36,0.000000,0.288000,0.500000,0.000000,1.571000,1.571000,3.585851,0.104785
N37,0.048000,0.234000,0.500000,1.570000,-2.680000,0.000000,3.673290,0.072250 N37,0.048000,0.234000,0.500000,0.000000,1.571000,1.571000,3.673290,0.072250
N38,0.198000,0.252000,0.500000,1.570000,-2.680000,0.000000,3.786958,0.151076 N38,0.198000,0.252000,0.500000,0.000000,1.571000,1.571000,3.786958,0.151076
N39,0.252000,0.306000,0.500000,1.570000,-2.680000,0.000000,3.881606,0.076368 N39,0.252000,0.306000,0.500000,0.000000,1.571000,1.571000,3.881606,0.076368
N40,0.264000,0.336000,0.500000,1.570000,-2.680000,0.000000,3.977590,0.032311 N40,0.264000,0.336000,0.500000,0.000000,1.571000,1.571000,3.977590,0.032311
N41,0.330000,0.234000,0.500000,1.570000,-2.680000,0.000000,4.082062,0.121491 N41,0.330000,0.234000,0.500000,0.000000,1.571000,1.571000,4.082062,0.121491
N42,0.330000,0.162000,0.500000,1.570000,-2.680000,0.000000,4.177567,0.072000 N42,0.330000,0.162000,0.500000,0.000000,1.571000,1.571000,4.177567,0.072000
N43,0.336000,0.102000,0.500000,1.570000,-2.680000,0.000000,4.284022,0.060299 N43,0.336000,0.102000,0.500000,0.000000,1.571000,1.571000,4.284022,0.060299
N44,0.396000,0.096000,0.500000,1.570000,-2.680000,0.000000,4.377034,0.060299 N44,0.396000,0.096000,0.500000,0.000000,1.571000,1.571000,4.377034,0.060299
N45,0.450000,0.144000,0.500000,1.570000,-2.680000,0.000000,4.494896,0.072250 N45,0.450000,0.144000,0.500000,0.000000,1.571000,1.571000,4.494896,0.072250
N46,0.456000,0.252000,0.500000,1.570000,-2.680000,0.000000,4.604838,0.108167 N46,0.456000,0.252000,0.500000,0.000000,1.571000,1.571000,4.604838,0.108167
N47,0.420000,0.300000,0.500000,1.570000,-2.680000,0.000000,4.684846,0.060000 N47,0.420000,0.300000,0.500000,0.000000,1.571000,1.571000,4.684846,0.060000
N48,0.378000,0.324000,0.500000,1.570000,-2.680000,0.000000,4.774341,0.048374 N48,0.378000,0.324000,0.500000,0.000000,1.571000,1.571000,4.774341,0.048374
N49,0.348000,0.324000,0.500000,1.570000,-2.680000,0.000000,4.876254,0.030000 N49,0.348000,0.324000,0.500000,0.000000,1.571000,1.571000,4.876254,0.030000

5
src/roboglue_ros/src/roboglue_recorder.cpp
View File

@@ -462,7 +462,7 @@ int main(int argc, char **argv) {
//////////////////////////////////////// ////////////////////////////////////////
bool openFile(internalState* is, fileData* fd, nlohmann::json meta){ bool openFile(internalState* is, fileData* fd, nlohmann::json meta){
if (!is->isFileOpen){ if (!is->isFileOpen && !meta["name"].get<std::string>().empty()){
// open metafile by name // open metafile by name
std::string metaInFilename = fd->meta_dir_name + meta["name"].get<std::string>() + fd->meta_ext; std::string metaInFilename = fd->meta_dir_name + meta["name"].get<std::string>() + fd->meta_ext;
ROS_DEBUG("Opening Meta File: [%s]", metaInFilename.c_str()); ROS_DEBUG("Opening Meta File: [%s]", metaInFilename.c_str());
@@ -530,6 +530,7 @@ bool openFile(internalState* is, fileData* fd, nlohmann::json meta){
return false; return false;
} }
} }
return false;
} }
bool closeFile(internalState* is, fileData* fd){ bool closeFile(internalState* is, fileData* fd){
@@ -544,7 +545,7 @@ bool closeFile(internalState* is, fileData* fd){
} }
fd->playVect->clear(); fd->playVect->clear();
fd->recordVect->clear(); fd->recordVect->clear();
} } return true;
} }
bool startRecord(internalState* is, fileData* fd, nlohmann::json meta){ bool startRecord(internalState* is, fileData* fd, nlohmann::json meta){

4
src/roboglue_ros/src/roboglue_utils.cpp
View File

@@ -105,8 +105,8 @@ geometry_msgs::PoseStamped twist2Pose(geometry_msgs::TwistStamped twt){
newPoseStamp.pose.position.x = twt.twist.linear.x; newPoseStamp.pose.position.x = twt.twist.linear.x;
newPoseStamp.pose.position.z = twt.twist.linear.z; newPoseStamp.pose.position.z = twt.twist.linear.z;
// FIXME ripristinare il calcolo con le coordinate di posa dal tracker, ora gli assi sono fissi // FIXME ripristinare il calcolo con le coordinate di posa dal tracker, ora gli assi sono fissi
//qt.setRPY(twPose.twist.angular.x, twPose.twist.angular.y, twPose.twist.angular.z); qt.setRPY(twt.twist.angular.x, twt.twist.angular.y, twt.twist.angular.z);
qt.setRPY(0, M_PI/2, M_PI/2); //qt.setRPY(0, M_PI/2, M_PI/2);
newPoseStamp.pose.orientation.x = qt.x(); newPoseStamp.pose.orientation.x = qt.x();
newPoseStamp.pose.orientation.y = qt.y(); newPoseStamp.pose.orientation.y = qt.y();
newPoseStamp.pose.orientation.z = qt.z(); newPoseStamp.pose.orientation.z = qt.z();