Beispiel #1
0
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;
}
Beispiel #2
0
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);
}
Beispiel #3
0
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;
}