23 #include "controller_openrave.h" 25 #include "exception.h" 27 #include <core/exceptions/software.h> 28 #include <plugins/openrave/aspect/openrave_connector.h> 31 # include <plugins/openrave/environment.h> 32 # include <plugins/openrave/manipulator.h> 33 # include <plugins/openrave/robot.h> 37 using namespace OpenRAVE;
60 KatanaControllerOpenrave::~KatanaControllerOpenrave()
71 KatanaControllerOpenrave::init()
75 OR_robot_ = openrave_->get_active_robot();
78 throw fawkes::Exception(
"Cannot access OpenRaveRobot in current OpenRaveEnvironment.");
83 OR_manip_ = OR_robot_->get_manipulator();
84 env_ = OR_env_->get_env_ptr();
85 robot_ = OR_robot_->get_robot_ptr();
86 manip_ = robot_->GetActiveManipulator();
89 }
catch (OpenRAVE::openrave_exception &e) {
95 KatanaControllerOpenrave::set_max_velocity(
unsigned int vel)
99 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
100 std::vector<dReal> v;
101 OR_manip_->get_angles(v);
102 v.assign(v.size(), (dReal)(vel / 100.0));
104 robot_->SetActiveDOFVelocities(v);
105 }
catch ( ::openrave_exception &e) {
111 KatanaControllerOpenrave::final()
114 return robot_->GetController()->IsDone();
118 KatanaControllerOpenrave::joint_angles()
123 KatanaControllerOpenrave::joint_encoders()
129 KatanaControllerOpenrave::calibrate()
135 KatanaControllerOpenrave::stop()
139 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
140 std::vector<dReal> v;
141 robot_->GetActiveDOFValues(v);
142 robot_->SetActiveDOFValues(v);
143 }
catch ( ::openrave_exception &e) {
149 KatanaControllerOpenrave::turn_on()
154 KatanaControllerOpenrave::turn_off()
159 KatanaControllerOpenrave::read_coordinates(
bool refresh)
163 update_manipulator();
164 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
165 Transform tf = manip_->GetEndEffectorTransform();
170 TransformMatrix m = matrixFromQuat(tf.rot);
171 std::vector<dReal> v;
172 OR_manip_->get_angles_device(v);
173 phi_ = v.at(0) - 0.5 * M_PI;
174 psi_ = 0.5 * M_PI - v.at(4);
175 theta_ = acos(m.m[10]);
177 if (asin(m.m[2] / sin(phi_)) < 0.0)
179 }
catch ( ::openrave_exception &e) {
185 KatanaControllerOpenrave::read_motor_data()
191 KatanaControllerOpenrave::read_sensor_data()
197 KatanaControllerOpenrave::gripper_open(
bool blocking)
202 KatanaControllerOpenrave::gripper_close(
bool blocking)
207 KatanaControllerOpenrave::move_to(
float x,
220 KatanaControllerOpenrave::move_to(std::vector<int> encoders,
bool blocking)
226 KatanaControllerOpenrave::move_to(std::vector<float> angles,
bool blocking)
230 std::vector<dReal> v;
231 OR_manip_->set_angles_device(angles);
233 OR_manip_->get_angles(v);
234 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
235 robot_->SetActiveDOFValues(v);
237 }
catch ( ::openrave_exception &e) {
247 KatanaControllerOpenrave::move_motor_to(
unsigned short id,
int enc,
bool blocking)
253 KatanaControllerOpenrave::move_motor_to(
unsigned short id,
float angle,
bool blocking)
257 std::vector<dReal> v;
258 OR_manip_->get_angles_device(v);
259 v.at(
id) = (dReal)angle;
260 OR_manip_->set_angles_device(v);
262 OR_manip_->get_angles(v);
263 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
264 robot_->SetActiveDOFValues(v);
266 }
catch ( ::openrave_exception &e) {
276 KatanaControllerOpenrave::move_motor_by(
unsigned short id,
int enc,
bool blocking)
282 KatanaControllerOpenrave::move_motor_by(
unsigned short id,
float angle,
bool blocking)
286 std::vector<dReal> v;
287 OR_manip_->get_angles_device(v);
288 v.at(
id) += (dReal)angle;
289 OR_manip_->set_angles_device(v);
291 OR_manip_->get_angles(v);
292 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
293 robot_->SetActiveDOFValues(v);
295 }
catch ( ::openrave_exception &e) {
306 KatanaControllerOpenrave::x()
312 KatanaControllerOpenrave::y()
318 KatanaControllerOpenrave::z()
324 KatanaControllerOpenrave::phi()
330 KatanaControllerOpenrave::theta()
336 KatanaControllerOpenrave::psi()
342 KatanaControllerOpenrave::get_sensors(std::vector<int> &to,
bool refresh)
350 KatanaControllerOpenrave::get_encoders(std::vector<int> &to,
bool refresh)
356 KatanaControllerOpenrave::get_angles(std::vector<float> &to,
bool refresh)
360 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
361 std::vector<dReal> v;
362 robot_->GetActiveDOFValues(v);
363 OR_manip_->set_angles(v);
365 OR_manip_->get_angles_device(to);
366 }
catch ( ::openrave_exception &e) {
372 KatanaControllerOpenrave::update_manipulator()
374 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
375 manip_ = robot_->GetActiveManipulator();
379 KatanaControllerOpenrave::wait_finished()
387 KatanaControllerOpenrave::motor_oor(
unsigned short id)
390 std::vector<dReal> v;
391 OR_manip_->get_angles_device(v);
393 return id > v.size();
397 KatanaControllerOpenrave::check_init()
405 #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.