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