//compute interpolation
Transform3D<> computeinterpolation(Transform3D<> Tin, Transform3D<> Tdes,double delta)
{
//number of steps, considering ti=1, ti-1=0, each step is 1/n
int n=0;
double WS=1.0;
Transform3D<> Tout;
//compute positional difference
Vector3D<> dif=Tdes.P()-Tin.P();
//Compute Wtotal
Rotation3D<> forW =inverse(Tin.R())*Tdes.R();
//Compute W vector
Vector3D<> W=getW(forW);
//Get the number of steps necessary
Vector3D<> trans, rot;
// trans=(1.0/n)*dif;
// rot=(1.0/n)*W;
double step;
do
{n=n+1;
trans=(1.0/n)*dif;
rot=(1.0/n)*W;
step=trans.norm2()/WS+rot.norm2()/pi;
}
while (step>delta);
n=1; //number of steps
EAA<> eaa(rot*n);
Transform3D<> current(Tin.P()+trans*n,Tin.R()*eaa.toRotation3D());
Tout=current;
return Tout;
}
//compute the small iteration
VelocityScrew6D<> computeG(Transform3D<> Tin, Transform3D<> Tdes){
//compute positional difference
Vector3D<> dif=Tdes.P()-Tin.P();
//compute rotation difference
Rotation3D<> rot=Tdes.R()*inverse(Tin.R());
//assign values to deltau
VelocityScrew6D<> deltau(dif(0),dif(1),dif(2),(rot(2,1)-rot(1,2))/2,(rot(0,2)-rot(2,0))/2,(rot(1,0)-rot(0,1))/2);
return(deltau);
}
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) {
//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;
}
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;
}
}
void transformToText(string pre, Transform3D<double> T){
RPY<double> rpy = RPY<double>(T.R());
ROS_INFO("xyz (%0.2f, %0.2f, %0.2f), RPY (%0.2f, %0.2f, %0.2f)", T.P()[0], T.P()[1], T.P()[2], rpy(0), rpy(1), rpy(2));
}
