C++ Transform3D::P方法代码示例

//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;
//.........这里部分代码省略.........

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));
}