23#include "controller_openrave.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>
37using namespace OpenRAVE;
60KatanaControllerOpenrave::~KatanaControllerOpenrave()
74 OR_env_ = openrave_->get_environment();
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) {
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) {
114 return robot_->GetController()->IsDone();
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) {
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) {
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) {
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) {
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) {
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) {
372KatanaControllerOpenrave::update_manipulator()
374 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
375 manip_ = robot_->GetActiveManipulator();
379KatanaControllerOpenrave::wait_finished()
387KatanaControllerOpenrave::motor_oor(
unsigned short id)
390 std::vector<dReal> v;
391 OR_manip_->get_angles_device(v);
393 return id > v.size();
397KatanaControllerOpenrave::check_init()
Base class for exceptions in Fawkes.
virtual double phi()=0
Get x-coordinate of latest endeffector position.
virtual void gripper_open(bool blocking=false)=0
Open Gripper.
virtual bool joint_angles()=0
Check if controller provides joint angle values.
virtual void read_motor_data()=0
Read motor data of currently active joints from device into controller libray.
virtual void set_max_velocity(unsigned int vel)=0
Set maximum velocity.
virtual double theta()=0
Get theta-rotation of latest endeffector orientation.
virtual void gripper_close(bool blocking=false)=0
Close Gripper.
virtual void read_sensor_data()=0
Read all sensor data from device into controller libray.
virtual double psi()=0
Get psi-rotation of latest endeffector orientation.
virtual double y()=0
Get y-coordinate of latest endeffector position.
virtual void init()=0
Initialize controller.
virtual double z()=0
Get z-coordinate of latest endeffector position.
virtual void move_motor_to(unsigned short id, int enc, bool blocking=false)=0
Move single joint/motor to encoder value.
virtual void move_motor_by(unsigned short id, int enc, bool blocking=false)=0
Move single joint/motor by encoder value (i.e.
virtual bool joint_encoders()=0
Check if controller provides joint encoder values.
virtual double x()=0
Get x-coordinate of latest endeffector position.
virtual bool final()=0
Check if movement is final.
virtual void turn_on()=0
Turn on arm/motors.
virtual void get_sensors(std::vector< int > &to, bool refresh=false)=0
Get sensor values.
virtual void calibrate()=0
Calibrate the arm.
virtual void get_encoders(std::vector< int > &to, bool refresh=false)=0
Get encoder values of joints/motors.
virtual void stop()=0
Stop movement immediately.
virtual void move_to(float x, float y, float z, float phi, float theta, float psi, bool blocking=false)=0
Move endeffctor to given coordinates.
virtual void get_angles(std::vector< float > &to, bool refresh=false)=0
Get angle values of joints/motors.
virtual void read_coordinates(bool refresh=false)=0
Store current coordinates of endeeffctor.
virtual void turn_off()=0
Turn off arm/motors.
Interface for a OpenRave connection creator.
Fawkes library namespace.