23 #include "controller_openrave.h" 24 #include "exception.h" 26 #include <core/exceptions/software.h> 29 #include <plugins/openrave/environment.h> 30 #include <plugins/openrave/robot.h> 31 #include <plugins/openrave/manipulator.h> 56 __openrave = openrave;
60 __initialized =
false;
64 KatanaControllerOpenrave::~KatanaControllerOpenrave()
76 KatanaControllerOpenrave::init()
80 __OR_robot = __openrave->get_active_robot();
82 if( __OR_robot == NULL) {
83 throw fawkes::Exception(
"Cannot access OpenRaveRobot in current OpenRaveEnvironment.");
88 __OR_manip = __OR_robot->get_manipulator();
89 __env = __OR_env->get_env_ptr();
90 __robot = __OR_robot->get_robot_ptr();
91 __manip = __robot->GetActiveManipulator();
94 }
catch (OpenRAVE::openrave_exception &e) {
100 KatanaControllerOpenrave::set_max_velocity(
unsigned int vel)
104 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
105 std::vector<dReal> v;
106 __OR_manip->get_angles(v);
107 v.assign(v.size(), (dReal)(vel / 100.0));
109 __robot->SetActiveDOFVelocities(v);
110 }
catch( ::openrave_exception &e) {
117 KatanaControllerOpenrave::final()
120 return __robot->GetController()->IsDone();
124 KatanaControllerOpenrave::joint_angles()
129 KatanaControllerOpenrave::joint_encoders()
135 KatanaControllerOpenrave::calibrate()
141 KatanaControllerOpenrave::stop()
145 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
146 std::vector<dReal> v;
147 __robot->GetActiveDOFValues(v);
148 __robot->SetActiveDOFValues(v);
149 }
catch( ::openrave_exception &e) {
155 KatanaControllerOpenrave::turn_on()
160 KatanaControllerOpenrave::turn_off()
165 KatanaControllerOpenrave::read_coordinates(
bool refresh)
169 update_manipulator();
170 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
171 Transform tf = __manip->GetEndEffectorTransform();
176 TransformMatrix m = matrixFromQuat(tf.rot);
177 std::vector<dReal> v;
178 __OR_manip->get_angles_device(v);
179 __phi = v.at(0) - 0.5*M_PI;
180 __psi = 0.5*M_PI - v.at(4);
181 __theta = acos(m.m[10]);
183 if( asin(m.m[2] / sin(__phi)) < 0.0 )
185 }
catch( ::openrave_exception &e ) {
191 KatanaControllerOpenrave::read_motor_data()
197 KatanaControllerOpenrave::read_sensor_data()
203 KatanaControllerOpenrave::gripper_open(
bool blocking)
209 KatanaControllerOpenrave::gripper_close(
bool blocking)
215 KatanaControllerOpenrave::move_to(
float x,
float y,
float z,
float phi,
float theta,
float psi,
bool blocking)
222 KatanaControllerOpenrave::move_to(std::vector<int> encoders,
bool blocking)
228 KatanaControllerOpenrave::move_to(std::vector<float> angles,
bool blocking)
232 std::vector<dReal> v;
233 __OR_manip->set_angles_device(angles);
235 __OR_manip->get_angles(v);
236 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
237 __robot->SetActiveDOFValues(v);
239 }
catch( ::openrave_exception &e) {
249 KatanaControllerOpenrave::move_motor_to(
unsigned short id,
int enc,
bool blocking)
255 KatanaControllerOpenrave::move_motor_to(
unsigned short id,
float angle,
bool blocking)
259 std::vector<dReal> v;
260 __OR_manip->get_angles_device(v);
261 v.at(
id) = (dReal)angle;
262 __OR_manip->set_angles_device(v);
264 __OR_manip->get_angles(v);
265 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
266 __robot->SetActiveDOFValues(v);
268 }
catch( ::openrave_exception &e) {
278 KatanaControllerOpenrave::move_motor_by(
unsigned short id,
int enc,
bool blocking)
284 KatanaControllerOpenrave::move_motor_by(
unsigned short id,
float angle,
bool blocking)
288 std::vector<dReal> v;
289 __OR_manip->get_angles_device(v);
290 v.at(
id) += (dReal)angle;
291 __OR_manip->set_angles_device(v);
293 __OR_manip->get_angles(v);
294 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
295 __robot->SetActiveDOFValues(v);
297 }
catch( ::openrave_exception &e) {
309 KatanaControllerOpenrave::x()
315 KatanaControllerOpenrave::y()
321 KatanaControllerOpenrave::z()
327 KatanaControllerOpenrave::phi()
333 KatanaControllerOpenrave::theta()
339 KatanaControllerOpenrave::psi()
345 KatanaControllerOpenrave::get_sensors(std::vector<int>& to,
bool refresh)
353 KatanaControllerOpenrave::get_encoders(std::vector<int>& to,
bool refresh)
359 KatanaControllerOpenrave::get_angles(std::vector<float>& to,
bool refresh)
363 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
364 std::vector<dReal> v;
365 __robot->GetActiveDOFValues(v);
366 __OR_manip->set_angles(v);
368 __OR_manip->get_angles_device(to);
369 }
catch( ::openrave_exception &e) {
376 KatanaControllerOpenrave::update_manipulator()
378 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
379 __manip = __robot->GetActiveManipulator();
383 KatanaControllerOpenrave::wait_finished()
391 KatanaControllerOpenrave::motor_oor(
unsigned short id)
394 std::vector<dReal> v;
395 __OR_manip->get_angles_device(v);
397 return id > v.size();
401 KatanaControllerOpenrave::check_init()
403 if( !__initialized ) {
409 #endif // HAVE_OPENRAVE Fawkes library namespace.
Base class for exceptions in Fawkes.
Interface for a OpenRave connection creator.
virtual OpenRaveEnvironment * get_environment() const =0
Get pointer to OpenRaveEnvironment object.