23 #include "controller_openrave.h" 24 #include "exception.h" 26 #include <core/exceptions/software.h> 28 #include <plugins/openrave/aspect/openrave_connector.h> 31 #include <plugins/openrave/environment.h> 32 #include <plugins/openrave/robot.h> 33 #include <plugins/openrave/manipulator.h> 58 __openrave = openrave;
59 __initialized =
false;
63 KatanaControllerOpenrave::~KatanaControllerOpenrave()
75 KatanaControllerOpenrave::init()
79 __OR_robot = __openrave->get_active_robot();
82 throw fawkes::Exception(
"Cannot access OpenRaveRobot in current OpenRaveEnvironment.");
87 __OR_manip = __OR_robot->get_manipulator();
88 __env = __OR_env->get_env_ptr();
89 __robot = __OR_robot->get_robot_ptr();
90 __manip = __robot->GetActiveManipulator();
93 }
catch (OpenRAVE::openrave_exception &e) {
99 KatanaControllerOpenrave::set_max_velocity(
unsigned int vel)
103 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
104 std::vector<dReal> v;
105 __OR_manip->get_angles(v);
106 v.assign(v.size(), (dReal)(vel / 100.0));
108 __robot->SetActiveDOFVelocities(v);
109 }
catch( ::openrave_exception &e) {
116 KatanaControllerOpenrave::final()
119 return __robot->GetController()->IsDone();
123 KatanaControllerOpenrave::joint_angles()
128 KatanaControllerOpenrave::joint_encoders()
134 KatanaControllerOpenrave::calibrate()
140 KatanaControllerOpenrave::stop()
144 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
145 std::vector<dReal> v;
146 __robot->GetActiveDOFValues(v);
147 __robot->SetActiveDOFValues(v);
148 }
catch( ::openrave_exception &e) {
154 KatanaControllerOpenrave::turn_on()
159 KatanaControllerOpenrave::turn_off()
164 KatanaControllerOpenrave::read_coordinates(
bool refresh)
168 update_manipulator();
169 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
170 Transform tf = __manip->GetEndEffectorTransform();
175 TransformMatrix m = matrixFromQuat(tf.rot);
176 std::vector<dReal> v;
177 __OR_manip->get_angles_device(v);
178 __phi = v.at(0) - 0.5*M_PI;
179 __psi = 0.5*M_PI - v.at(4);
180 __theta = acos(m.m[10]);
182 if( asin(m.m[2] / sin(__phi)) < 0.0 )
184 }
catch( ::openrave_exception &e ) {
190 KatanaControllerOpenrave::read_motor_data()
196 KatanaControllerOpenrave::read_sensor_data()
202 KatanaControllerOpenrave::gripper_open(
bool blocking)
208 KatanaControllerOpenrave::gripper_close(
bool blocking)
214 KatanaControllerOpenrave::move_to(
float x,
float y,
float z,
float phi,
float theta,
float psi,
bool blocking)
221 KatanaControllerOpenrave::move_to(std::vector<int> encoders,
bool blocking)
227 KatanaControllerOpenrave::move_to(std::vector<float> angles,
bool blocking)
231 std::vector<dReal> v;
232 __OR_manip->set_angles_device(angles);
234 __OR_manip->get_angles(v);
235 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
236 __robot->SetActiveDOFValues(v);
238 }
catch( ::openrave_exception &e) {
248 KatanaControllerOpenrave::move_motor_to(
unsigned short id,
int enc,
bool blocking)
254 KatanaControllerOpenrave::move_motor_to(
unsigned short id,
float angle,
bool blocking)
258 std::vector<dReal> v;
259 __OR_manip->get_angles_device(v);
260 v.at(
id) = (dReal)angle;
261 __OR_manip->set_angles_device(v);
263 __OR_manip->get_angles(v);
264 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
265 __robot->SetActiveDOFValues(v);
267 }
catch( ::openrave_exception &e) {
277 KatanaControllerOpenrave::move_motor_by(
unsigned short id,
int enc,
bool blocking)
283 KatanaControllerOpenrave::move_motor_by(
unsigned short id,
float angle,
bool blocking)
287 std::vector<dReal> v;
288 __OR_manip->get_angles_device(v);
289 v.at(
id) += (dReal)angle;
290 __OR_manip->set_angles_device(v);
292 __OR_manip->get_angles(v);
293 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
294 __robot->SetActiveDOFValues(v);
296 }
catch( ::openrave_exception &e) {
308 KatanaControllerOpenrave::x()
314 KatanaControllerOpenrave::y()
320 KatanaControllerOpenrave::z()
326 KatanaControllerOpenrave::phi()
332 KatanaControllerOpenrave::theta()
338 KatanaControllerOpenrave::psi()
344 KatanaControllerOpenrave::get_sensors(std::vector<int>& to,
bool refresh)
352 KatanaControllerOpenrave::get_encoders(std::vector<int>& to,
bool refresh)
358 KatanaControllerOpenrave::get_angles(std::vector<float>& to,
bool refresh)
362 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
363 std::vector<dReal> v;
364 __robot->GetActiveDOFValues(v);
365 __OR_manip->set_angles(v);
367 __OR_manip->get_angles_device(to);
368 }
catch( ::openrave_exception &e) {
375 KatanaControllerOpenrave::update_manipulator()
377 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
378 __manip = __robot->GetActiveManipulator();
382 KatanaControllerOpenrave::wait_finished()
390 KatanaControllerOpenrave::motor_oor(
unsigned short id)
393 std::vector<dReal> v;
394 __OR_manip->get_angles_device(v);
396 return id > v.size();
400 KatanaControllerOpenrave::check_init()
402 if( !__initialized ) {
408 #endif // HAVE_OPENRAVE Fawkes library namespace.
virtual OpenRaveEnvironmentPtr get_environment() const =0
Get pointer to OpenRaveEnvironment object.
Base class for exceptions in Fawkes.
Interface for a OpenRave connection creator.