//computes new configuration for urTobj Q computenewQ(Q qin, Transform3D<> Tin, Transform3D<> Tdes, Device::Ptr device, State state,Frame* marker,Frame* bottleframe){ //set device state //Q home(6, 1.6765, -2.0368, 1.5597, -1.6505, -1.6474, -1.6449); device->setQ(qin,state); //compute the small iteration, show error VelocityScrew6D<> deltau=computeG(Tin,Tdes); //do while error larger than threshold while (deltau.norm2()>epsilon && flag==0){ //get Jacobian Jacobian J=device->baseJframe(marker,state); //calcultate dq // Q dq (LinearAlgebra::inverse(J.e())*deltau.e()); Q dq (J.e().inverse()*deltau.e()); //add dq to q // Q dq(6,1,2,3,4,5,6); qin=qin+dq; //qin=0; //set new q device->setQ(qin,state); //get new transform Transform3D<> bThan = device->baseTframe(marker,state); Tin=bThan; //compute and show new deltau, error deltau=computeG(Tin,Tdes); T2=clock(); Ttotal=T2-T1; sec += (Ttotal/CLOCKS_PER_SEC); if(sec>30) { flag=1; } } //return final Q return(qin); }
bool receiveXYZRPY(kinematics::KinXYZRPY::Request &req, kinematics::KinXYZRPY::Response &res){ //ROS_INFO("xyz (%0.2f, %0.2f, %0.2f), RPY (%0.2f, %0.2f, %0.2f)", req.x, req.y, req.z, req.R, req.P, req.Y); if (req.from_current){ device->setQ(positions, state); } Transform3D<double> Tcurrent, Tdesired; Tcurrent = device->baseTend(state); Tdesired.P() = Vector3D<double>(req.x, req.y, req.z); Tdesired.R() = RPY<double>(req.R*Pi/180, req.P*Pi/180, req.Y*Pi/180).toRotation3D(); res.success = sendConfigurations(Tcurrent, Tdesired, state, req.interpolate, req.force_angle, req.angle); return true; }
void posCallback(const pa10_controller::Positions::ConstPtr& msg){ //ROS_INFO("I HEARD: %0.2f %0.2f %0.2f %0.2f %0.2f %0.2f %0.2f", msg->positions[0],msg->positions[1],msg->positions[2],msg->positions[3],msg->positions[4],msg->positions[5],msg->positions[6]); if (positions.size() == 0){ positions = Q(7,0,0,0,0,0,0,0); for (int i = 0; i < (int) msg->positions.size(); i ++){ positions[i] = msg->positions[i]*Pi/180; } device->setQ(positions, state); }else{ for (int i = 0; i < (int) msg->positions.size(); i ++){ positions[i] = msg->positions[i]*Pi/180; } } calculatePose(positions); }
bool checkCollisions(Device::Ptr device, const State &state, const CollisionDetector &detector, const Q &q) { State testState; CollisionDetector::QueryResult data; bool colFrom; testState = state; device->setQ(q,testState); colFrom = detector.inCollision(testState,&data); if (colFrom) { cerr << "Configuration in collision: " << q << endl; cerr << "Colliding frames: " << endl; FramePairSet fps = data.collidingFrames; for (FramePairSet::iterator it = fps.begin(); it != fps.end(); it++) { cerr << (*it).first->getName() << " " << (*it).second->getName() << endl; } return false; } return true; }
void calculatePose(Q q){ State tmpState = wc->getDefaultState(); device->setQ(q, tmpState); Transform3D<double> cPose = device->baseTend(tmpState); RPY<double> rpy(cPose.R()); pose.x = cPose.P()[0]; pose.y = cPose.P()[1]; pose.z = cPose.P()[2]; pose.R = rpy(0)*180/Pi; pose.P = rpy(1)*180/Pi; pose.Y = rpy(2)*180/Pi; //cout << "POOOOOSE: " << cPose.P() << "\n"; kinematics::Pose msg; msg.x = cPose.P()[0]; msg.y = cPose.P()[1]; msg.z = cPose.P()[2]; msg.R = rpy(0)*180/Pi; msg.P = rpy(1)*180/Pi; msg.Y = rpy(2)*180/Pi; pose_pub.publish(msg); }
bool receiveJog(kinematics::KinXYZRPY::Request &req, kinematics::KinXYZRPY::Response &res){ device->setQ(positions, state); cout << "POSITIONS: " << positions << "\n"; Transform3D<double> Tcurrent = device->baseTend(state); Transform3D<double> Tdesired = device->baseTend(state); Tdesired.P() += Vector3D<double>(req.x, req.y, req.z); //Rotation3D<double> rot = RPY<double>(req.R, req.P, req.Y).toRotation3D(); /* Tdesired.R() = Rotation3D<double>( Tcurrent.R(0,0)+rot(0,0), Tcurrent.R(0,1)+rot(0,1), Tcurrent.R(0,2)+rot(0,2), Tcurrent.R(1,0)+rot(1,0), Tcurrent.R(1,1)+rot(1,1), Tcurrent.R(1,2)+rot(1,2), Tcurrent.R(2,0)+rot(2,0), Tcurrent.R(2,1)+rot(2,1), Tcurrent.R(2,1)+rot(2,2), ); */ res.success = sendConfigurations(Tcurrent, Tdesired, state, req.interpolate); return true; }
int main(int argc, char **argv){ ros::init(argc, argv, "kinematics_node"); ROS_INFO("kinematics_node started"); ros::NodeHandle n, nh("~"); //string wcFile = "/home/ege/ros/sandbox/kinematics/Scene/Scene.wc.xml"; //string deviceName = "PA10"; string wcFile, deviceName, xyzrpyService, qService, posTopic, jogService, poseTopic; nh.param<string>("workcell", wcFile, "/home/ege/ros/sandbox/kinematics/Scene/Scene.wc.xml"); nh.param<string>("device", deviceName, "PA10"); nh.param<string>("xyzrpy_service", xyzrpyService, "/pa10_xyzRPY"); nh.param<string>("jog_service", jogService, "/pa10_jog"); nh.param<string>("q_service", qService, "/pa10_Qs"); nh.param<string>("pos_topic", posTopic, "/pa10_positions"); nh.param<string>("pose_topic", poseTopic, "/pa10_pose"); nh.param<double>("interpolate_stepsize", interpolateStepSize, 0.1); ROS_INFO("Trying to use workcell: %s and device %s" , wcFile.c_str(), deviceName.c_str()); wc=WorkCellLoader::load(wcFile); device=wc->findDevice(deviceName); state = wc->getDefaultState(); Q qStart(7,0,0,0,0,0,0,0); device->setQ(qStart, state); ros::Subscriber sub = n.subscribe(posTopic, 1, posCallback); pose_pub = n.advertise<kinematics::Pose>(poseTopic, 10); ros::service::waitForService(qService, ros::Duration(1.0)); serviceClient = n.serviceClient<pa10_controller::PA10Qs>(qService); ros::ServiceServer service = n.advertiseService(xyzrpyService, receiveXYZRPY); ros::ServiceServer jService = n.advertiseService(jogService, receiveJog); ROS_INFO("kinematics_node ready"); ros::spin(); return 0; }
int main(int argc, char** argv) { //get wcFile and device name string wcFile = "/home/sdsuvei/robwork/RobWorkStudio/scenes/COBLab/KitchenScene.wc.xml"; string robotName = "UR"; string gripperName = "SDH"; std::string connectorName = "URConnector"; cout << "Trying to use workcell: " << wcFile << " and device " << robotName << endl; //load work cell WorkCell::Ptr wc = WorkCellFactory::load(wcFile); Device::Ptr UR = wc->findDevice(robotName); Device::Ptr gripper = wc->findDevice(gripperName); Device::Ptr connector = wc->findDevice(connectorName); if (UR == NULL) { cerr << "Device: " << robotName << " not found!" << endl; return 0; } //default state of the RobWork Workcell State state = wc->getDefaultState(); //load the collision detector CollisionDetector cd(wc,ProximityStrategyFactory::makeDefaultCollisionStrategy()); //ROS initialization ros::init(argc, argv, "listener"); ros::NodeHandle nh; ros::Rate loop_rate(5); sub = nh.subscribe ("/joint_states", 1, jointCallback); while(ros::ok()) { if(itHappened) { //RobWork robot model initialization //starting configuration of the UR5 arm // Q pos(6,0.0785, -1.7667, 2.037, -2.1101, -2.1724, -1.8505); // Q pos(6,-0.3946, -1.8944, 1.9378, -1.3807, -1.1023, -2.4439); Q pos(6, joint_pos[0], joint_pos[1], joint_pos[2], joint_pos[3], joint_pos[4], joint_pos[5]); cout<<"Initial arm configuration:\n"<<pos<<"\n"; //set the initial position of the UR5 arm UR->setQ(pos,state); //starting configuration of the SDH gripper Q grip(7,-1.047,0.000,0.000,-0.765,-0.282,-0.846,0); //set the initial position of the gripper gripper->setQ(grip,state); //half-away configuration of the URconnector Q conn(1,-1.597); //set the half-away configuration of the URconnector connector->setQ(conn,state); //RobWork Workcell frame definitions // Frame* bottle = wc->findFrame("BottleTray"); //bootle frame MovableFrame* bottle = (MovableFrame*)wc->findFrame("BottleTray"); Frame* marker=wc->findFrame("SDH.Marker"); //marker frame Frame* camera=wc->findFrame("Camera3D"); //camera frame Frame* pregrasp=wc->findFrame("Pregrasp"); // pregrasping frame if(cd.inCollision(state)) { cout<<"Before change: In collision!\n"; } //update the bottle frame with the value from the tracker Transform3D<> camTobj_rw=rw_camTobj(UR,state,bottle,camera); //change the object frame in the RobWork scene bottle->setTransform(camTobj_rw,state); if(cd.inCollision(state)) { cout<<"After change: In collision!\n"; } //store the kinematic values, from the RobWork workcell Transform3D<> kinematic_camTobj=Kinematics::frameTframe(camera,bottle,state); Transform3D<> kinematic_camTmrk=Kinematics::frameTframe(camera,marker,state); //compute the intermediate transforms // 1)URbase -> MARKER transform Transform3D<> urTmrk = UR->baseTframe(marker,state); // 2)CAMERA -> OBJECT transform Transform3D<> camTobj=find_camTobj(UR,state,bottle,camera,kinematic_camTobj); // 3)CAMERA -> MARKER transform Transform3D<> camTmrk=find_camTmrk(UR,state,marker,camera,kinematic_camTmrk); // 4)MARKER -> OBJECT transform Transform3D<> mrkTobj=inverse(camTmrk)*camTobj; // 5)OBJECT -> PREGRASP transform // Transform3D<> objTpg=Kinematics::frameTframe(bottle,pregrasp,state); // 6)URbase -> PreGrasping frame Transform3D<> objTpg=Kinematics::frameTframe(bottle,pregrasp,state); Transform3D<> urTcam=find_urTcam(UR,state,camera); Transform3D<> urTpg=urTcam*camTobj_rw*objTpg; //take initial rotation of object as pregrasping rotation // Transform3D<> urTpg=urTcam*camTobj_rw; // 7)URbase -> OBJECT transform Transform3D<> urTobj((urTmrk*mrkTobj*objTpg).P(),urTpg.R()); // Transform3D<> urTobj((urTmrk*mrkTobj).P(),urTpg.R()); //determine initial distance between the gripper-marker and the object Vector3D<> dist=(urTobj.P()-urTmrk.P()); cout<<"Initial distance is: "<<dist.norm2()<<endl; cout<<endl; //set the admissible threshold - distance between gripper-marker and object double tresh=0.01; //store the current value kinematic_camTmrk=camTmrk; kinematic_camTobj=camTobj; //start servoing method while (!cd.inCollision(state)){ //compute current distance between gripper-marker and object Vector3D<> cur_dist=(urTobj.P()-urTmrk.P()); cout<<"Current distance: "<<cur_dist.norm2()<<"\n"; //stop program if threshold limit is exceeded if(cur_dist.norm2()<tresh) { cout<<"Threshold limit exceeded!"<<endl; ros::shutdown(); break; } //compute the new urTmrk transform by performing interpolation between urTmrk & urTobj Transform3D<> urTmrk_new=computeinterpolation(urTmrk,urTobj,deltatess); //compute the new UR5 configuration which moves the gripper-marker closer to the object T1=clock(); Q new_pos=computenewQ(pos,urTmrk,urTmrk_new,UR,state,marker,bottle); //new configuration //Bring robot in new configuration UR->setQ(new_pos,state); //set device cout<<"new_pos:"<<new_pos; cout<<endl; //store the new urTmrk transform urTmrk=urTmrk_new; //store the new UR5 arm configuration pos=new_pos; //recompute the intermediate transforms // 1)CAMERA -> OBJECT transform Transform3D<> camTobj=find_camTobj(UR,state,bottle,camera,kinematic_camTobj); // 2)CAMERA -> MARKER transform Transform3D<> camTmrk=find_camTmrk(UR,state,marker,camera,kinematic_camTmrk); // 3)MARKER -> OBJECT transform Transform3D<> mrkTobj=inverse(camTmrk)*camTobj; // 4)URbase -> OBJECT transform // Transform3D<> urTobj((urTmrk*mrkTobj*objTpg).P(),urTpg.R()); //use the pregrasping rotation Transform3D<> urTobj((urTmrk*mrkTobj).P(),urTpg.R()); //store kinematic values for camTobj & camTmrk kinematic_camTobj=camTobj; kinematic_camTmrk=camTmrk; //execute robot movement //open file to write the script std::ofstream myfile; cout << "Generating script ... "; myfile.open ("/home/sdsuvei/workspace/PythonScripts/armQ.py"); if (myfile.is_open()){ myfile << "#!/usr/bin/python \n \n" << "import roslib \n" << "roslib.load_manifest('cob_script_server') \n" << "import rospy \n \n" << "from simple_script_server import * \n" << "sss = simple_script_server() \n \n" << "if __name__ == \"__main__\": \n" << " rospy.init_node(\"asd\") \n" << " sss.move(\"arm\", [["<<pos[0]<<","<<pos[1]<<","<<pos[2]<<","<<pos[3]<<","<<pos[4]<<","<<pos[5]<<"]]) \n"; myfile.close(); cout << "Done!" << endl; } else{ cout << "myfile not open!" << endl; } int val1; cout<<"\n Press key to allow movement: "; cin>>val1; cout<<"\n"; cout << "Executing movement..." << endl; system("python /home/sdsuvei/workspace/PythonScripts/armQ.py"); cout << "Movement done!" << endl; int val; cout<<"\n Press any key to continue "; cin>>val; cout<<"\n"; } itHappened=false; } ros::spinOnce(); loop_rate.sleep(); } cout<<"After While"; cout<<endl; if(flag) { cout<<"\nOrientation impossible! Try a different grasping approach!\n"<< endl; } else { cout<<"\nDone!\n\n"; } return 0; }
int main(int argc, char** argv) { const string wcFile = "Kr16WallWorkCell/Scene.wc.xml"; const string deviceName = "KukaKr16"; cout << "Trying to use workcell " << wcFile << " and device " << deviceName << endl; WorkCell::Ptr wc = WorkCellLoader::Factory::load(wcFile); Device::Ptr device = wc->findDevice(deviceName); if (device == NULL) { cerr << "Device: " << deviceName << " not found!" << endl; return 0; } const char* stats_str; double extend; if (argc > 1) { stats_str = argv[1]; extend = atof(argv[2]); }else { stats_str = "stat.txt"; extend = 0.1; } cout << extend << endl << stats_str <<endl; Math::seed(); State state = wc->getDefaultState(); Q start(6,-3.142,-0.827,-3.002,-3.143,0.099,-1.573); device->setQ(start,state); MovableFrame* bottle = wc->findFrame<MovableFrame>("Bottle"); Kinematics::gripFrame(bottle,device->getEnd(),state); CollisionDetector detector(wc, ProximityStrategyFactory::makeDefaultCollisionStrategy()); PlannerConstraint constraint = PlannerConstraint::make(&detector,device,state); /** Most easy way: uses default parameters based on given device sampler: QSampler::makeUniform(device) metric: PlannerUtil::normalizingInfinityMetric(device->getBounds()) extend: 0.05 */ //QToQPlanner::Ptr planner = RRTPlanner::makeQToQPlanner(constraint, device, RRTPlanner::RRTConnect); /** More complex way: allows more detailed definition of parameters and methods */ QSampler::Ptr sampler = QSampler::makeConstrained(QSampler::makeUniform(device),constraint.getQConstraintPtr()); QMetric::Ptr metric = MetricFactory::makeEuclidean<Q>(); //double extend = 0.1; Q from(6,-3.142,-0.827,-3.002,-3.143,0.099,-1.573); //Q to(6,1.7,0.6,-0.8,0.3,0.7,-0.5); // Very difficult for planner Q to(6,1.571,0.006,0.030,0.153,0.762,4.490); if (!checkCollisions(device, state, detector, from)) return 0; if (!checkCollisions(device, state, detector, to)) return 0; cout << "Planning from " << from << " to " << to << endl; ofstream stat; stat.open(stats_str); stat << "Path length,Time" << endl; QPath path; Timer t; while (extend <= 0.5) { QToQPlanner::Ptr planner = RRTPlanner::makeQToQPlanner(constraint, sampler, metric, extend, RRTPlanner::RRTConnect); t.resetAndResume(); planner->query(from,to,path,MAXTIME); t.pause(); stat << path.size() << "," << t.getTime() << "," << extend << endl; for (int x=1; x<100; x++) { QPath path2; t.resetAndResume(); planner->query(from,to,path2,MAXTIME); t.pause(); stat << path2.size() << "," << t.getTime() << "," << extend << endl; if (path2.size() < path.size()) { path = path2; } } extend += 0.01; } if (t.getTime() >= MAXTIME) { cout << "Notice: max time of " << MAXTIME << " seconds reached." << endl; } ofstream LUA; //create file object LUA.open("LUA.txt"); //open the file //write the LUA "header" LUA << "wc = rws.getRobWorkStudio():getWorkCell()\n"; LUA << "state = wc:getDefaultState() \ndevice = wc:findDevice(\"KukaKr16\")\n"; LUA << "bottle = wc:findFrame(\"Bottle\")\n"; LUA << "function setQ(q)\n"; LUA << " qq = rw.Q(#q,q[1],q[2],q[3],q[4],q[5],q[6])\n"; LUA << " device:setQ(qq,state)\n"; LUA << " rws.getRobWorkStudio():setState(state)\n"; LUA << " rw.sleep(0.1)\n"; LUA << "end\n"; LUA << "setQ({-3.142, -0.827, -3.002, -3.143, 0.099, -1.573})\n"; LUA << "rw.gripFrame(bottle,device:getEnd(),state)\n"; for (QPath::iterator it = path.begin(); it < path.end(); it++) { //cout << *it << endl; //output path to Terminal ostringstream out; out << *it; //write path to ostring string out2 = out.str(); //convert to string LUA << "setQ(" << out2.substr(4,out2.length()) << ")" << endl; // write path to LUA file. } LUA.close(); //close file cout << "Program done." << endl; return 0; }
bool sendConfigurations(Transform3D<double> Tcurrent, Transform3D<double> Tdesired, State& curState, bool interpolate=false, bool force_angle=false, double angle=0.0){ ROS_INFO("MAKING XYZ RPY"); transformToText("From", Tcurrent); transformToText("To", Tdesired); //cout << "From " << Tcurrent.P() << "\n"; //cout << "To " << Tdesired.P() << "\n"; rw::invkin::JacobianIKSolver solver(device, curState); solver.setCheckJointLimits(true); solver.setClampToBounds(true); Q qDesired; vector<Q> path; double length = (Tdesired.P()-Tcurrent.P()).norm2(); if (interpolate && length > interpolateStepSize){ solver.setEnableInterpolation(true); double stepSize = interpolateStepSize; cout << "Norm2: " << length << endl; rw::trajectory::LinearInterpolator<Transform3D<double> > interpolator(Tcurrent,Tdesired,length); double while_t = 0; while(while_t <= length){ double t = while_t; //for (double t=stepSize; t <= length; t+=stepSize){ cout << "T at: " << t << " : " << interpolator.x(t).P() << "\n"; vector<Q> newQs = solver.solve(interpolator.x(t), curState); if (newQs.size()>0){ qDesired = newQs[0] *180 / Pi; path.push_back(qDesired); device->setQ(newQs[0], curState); cout << "Q at: " << t << " : " << qDesired << "\n"; }else{ cout << "Couldn't interpolate " << Tdesired << "\n"; break; } while_t += stepSize; } } vector<Q> newQs = solver.solve(Tdesired, curState); cout << "Last T: " << Tdesired.P() << "\n"; if (newQs.size()>0){ qDesired = newQs[0] * 180 / Pi; //cout << "Q: " << qDesired << " - " << newQs[0] << "\n"; path.push_back(qDesired); device->setQ(newQs[0], curState); cout << "last Q: " << qDesired << "\n"; //cout << "AFTER: " << endl; //transformToText("After: ", device->baseTend(curState)); //cout << "AFTER : " << device->baseTend(curState).P() << "\n"; //cout << "AFTER : " << device->baseTend(state).P() << "\n"; } if (path.size() > 0){ for (int i = 0; i < (int) path.size(); i++){ if (force_angle){ path[i][6] += angle; } sendQs(path[i]); } return true; }else{ return false; } }
tuple<double, double, double> pathPlannerFunc(double extend){ const string wcFile = "/home/student/ROVI/Mand2/Kr16WallWorkCell/Scene.wc.xml"; const string deviceName = "KukaKr16"; const string bottle = "Bottle"; Q from(6,-3.142,-0.827,-3.002,-3.143,0.099,-1.573); Q to(6,1.571,0.006,0.030,0.153,0.762,4.490); WorkCell::Ptr wc = WorkCellLoader::Factory::load(wcFile); Device::Ptr device = wc->findDevice(deviceName); rw::kinematics::Frame *deviceB = wc->findFrame(bottle); if (device == NULL) { cerr << "Device: " << deviceName << " not found!" << endl; exit(-1); } if (deviceB == NULL) { cerr << "Device: " << bottle << " not found!" << endl; exit(-1); } rw::kinematics::State state = wc->getDefaultState(); device->setQ(from, state); Kinematics::gripFrame(deviceB,device->getEnd(),state); CollisionDetector detector(wc, ProximityStrategyFactory::makeDefaultCollisionStrategy()); PlannerConstraint constraint = PlannerConstraint::make(&detector,device,state); QSampler::Ptr sampler = QSampler::makeConstrained(QSampler::makeUniform(device),constraint.getQConstraintPtr()); QMetric::Ptr metric = MetricFactory::makeEuclidean<Q>(); QToQPlanner::Ptr planner = RRTPlanner::makeQToQPlanner(constraint, sampler, metric, extend, RRTPlanner::RRTConnect); if (!checkCollisions(device, state, detector, from)) exit(-1); if (!checkCollisions(device, state, detector, to)) exit(-1); PathAnalyzer::CartesianAnalysis distance_traveled; double path_length = 0; PathAnalyzer path_analyse(device,state); QPath path; Timer t; t.resetAndResume(); planner->query(from,to,path,MAXTIME); t.pause(); distance_traveled = path_analyse.analyzeCartesian(path,deviceB); path_length += distance_traveled.length; cout << extend<< "\tPath of length " << path.size() << " found in " << t.getTime() << " seconds." << endl; if (t.getTime() >= MAXTIME) { cout << "Notice: max time of " << MAXTIME << " seconds reached." << endl; } for (QPath::iterator it = path.begin(); it < path.end(); it++) { cout << "set" << *it << endl;//"setQ" << bins.substr(3,bins.length()) << endl; } auto tmp = make_tuple(extend, path_length,t.getTime()); return tmp; }