From 5acbdc128768a7c48d2cea7593a773547ebfad38 Mon Sep 17 00:00:00 2001 From: philipp schoenberger Date: Mon, 24 Aug 2015 17:56:36 +0200 Subject: [PATCH] cleanup big time --- .gitignore | 1 + lwrserv/Makefile | 51 ++- lwrserv/doc/doxygenconfig | 7 + lwrserv/include/BangBangTrajectory.h | 2 +- lwrserv/include/LinearTrajectory.h | 3 +- lwrserv/include/{mat.h => Mat.h} | 2 +- lwrserv/include/{robot.h => Robot.h} | 62 +-- lwrserv/{ => include}/SocketObject.h | 0 lwrserv/{ => include}/StringTool.h | 0 lwrserv/include/SvrData.h | 74 ++-- lwrserv/{ => include}/SvrHandling.h | 0 lwrserv/include/Trajectroy.h | 5 +- lwrserv/include/{vec.h => Vec.h} | 2 +- lwrserv/include/commands.h | 11 +- lwrserv/{ => include}/config.h | 1 - lwrserv/include/defines.h | 6 +- lwrserv/{ => include}/friComm.h | 0 lwrserv/{ => include}/friremote.h | 0 lwrserv/{ => include}/friudp.h | 0 lwrserv/include/global.h | 34 -- lwrserv/include/lwr4.h | 6 +- lwrserv/program.cpp | 585 --------------------------- lwrserv/{ => src}/SocketObject.cpp | 0 lwrserv/{ => src}/StringTool.cpp | 0 lwrserv/{ => src}/SvrData.cpp | 109 ++--- lwrserv/{ => src}/SvrHandling.cpp | 23 +- lwrserv/{ => src}/commands.cpp | 373 ++++------------- lwrserv/{ => src}/friremote.cpp | 0 lwrserv/{ => src}/friudp.cpp | 0 lwrserv/src/main.cpp | 183 +++++++++ lwrserv/src/program.cpp | 177 ++++++++ 31 files changed, 620 insertions(+), 1097 deletions(-) create mode 100644 lwrserv/doc/doxygenconfig rename lwrserv/include/{mat.h => Mat.h} (96%) rename lwrserv/include/{robot.h => Robot.h} (54%) rename lwrserv/{ => include}/SocketObject.h (100%) rename lwrserv/{ => include}/StringTool.h (100%) mode change 100755 => 100644 rename lwrserv/{ => include}/SvrHandling.h (100%) mode change 100755 => 100644 rename lwrserv/include/{vec.h => Vec.h} (95%) rename lwrserv/{ => include}/config.h (99%) rename lwrserv/{ => include}/friComm.h (100%) rename lwrserv/{ => include}/friremote.h (100%) rename lwrserv/{ => include}/friudp.h (100%) delete mode 100644 lwrserv/include/global.h delete mode 100644 lwrserv/program.cpp rename lwrserv/{ => src}/SocketObject.cpp (100%) rename lwrserv/{ => src}/StringTool.cpp (100%) mode change 100755 => 100644 rename lwrserv/{ => src}/SvrData.cpp (80%) rename lwrserv/{ => src}/SvrHandling.cpp (88%) mode change 100755 => 100644 rename lwrserv/{ => src}/commands.cpp (52%) rename lwrserv/{ => src}/friremote.cpp (100%) rename lwrserv/{ => src}/friudp.cpp (100%) create mode 100644 lwrserv/src/main.cpp create mode 100644 lwrserv/src/program.cpp diff --git a/.gitignore b/.gitignore index 1909db2..073cf19 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ build Release +html diff --git a/lwrserv/Makefile b/lwrserv/Makefile index 404e3d0..64e56b2 100644 --- a/lwrserv/Makefile +++ b/lwrserv/Makefile @@ -8,12 +8,13 @@ ################################################################################ LIBS = pthread boost_thread boost_system TOP_DIR = $(PWD) -TEST_DIR = $(BUILD_DIR)/test +SRC_FOLDER = ./src +TEST_DIR = ./test INCLUDE_DIR = -I . \ -I ./include # comment this out if you want debug compile info -Q=@ +#Q=@ ifeq ($(MAKECMDGOALS),test) GCOV := --coverage @@ -36,13 +37,13 @@ ifeq ($(MAKECMDGOALS),test) SRC_CPP := $(wildcard $(TEST_SRC_FOLDER)/*.cpp) SRC_HPP := $(wildcard $(TEST_SRC_FOLDER)/*.hpp) else -BUILD_DIR = $(PWD)/build/app +BUILD_DIR = $(TOP_DIR)/build/app # Sources -SRC_C := $(wildcard *.c) -SRC_CPP := $(wildcard *.cpp) +SRC_C += $(wildcard $(SRC_FOLDER)/*.c) +SRC_CPP += $(wildcard $(SRC_FOLDER)/*.cpp) endif -SRC_H += $(wildcard *.h) $(wildcard include/*.h) -SRC_HPP += $(wildcard *.hpp) +SRC_H += $(wildcard $(INCLUDE_DIR: % = %.h) $(SRC_FOLDER: % = %.h)) +SRC_HPP += $(wildcard $(INCLUDE_DIR: % = %.hpp) $(SRC_FOLDER: % = %.hpp)) OUT_BINARY = $(APP_PREFIX)lwrserv @@ -51,29 +52,36 @@ GLOBAL_FLAGS = -g -o2 -ansi -pedantic -DHAVE_SOCKLEN_T $(INCLUDE_DIR) CFLAGS = $(GLOBAL_FLAGS) $(CFLAGS_TEST) CXXFLAGS = $(GLOBAL_FLAGS) $(CFLAGS_TEST) CPPFLAGS = $(GLOBAL_FLAGS) $(CFLAGS_TEST) -LDFLAGS = $(patsubst %,-l%,$(LIBS)) $(CFLAGS_TEST) $(LIBS_TEST) +LDFLAGS = $(patsubst %,-l%,$(LIBS)) $(CFLAGS_TEST) $(LIBS_TEST) # Objects -OBJS_C = $(patsubst %.c,$(BUILD_DIR)/%.o,$(SRC_C)) -OBJS_CPP = $(patsubst %.cpp,$(BUILD_DIR)/%.o,$(SRC_CPP)) - - -OBJS=$(patsubst %.o,%.o,$(OBJS_C) $(OBJS_CPP)) +OBJS_C = $(patsubst %.c, $(BUILD_DIR)/%.o, $(SRC_C)) +OBJS_CPP = $(patsubst %.cpp, $(BUILD_DIR)/%.o, $(SRC_CPP)) +OBJS = $(patsubst %.o, %.o, $(OBJS_C) $(OBJS_CPP)) # Dependencies -DEPS_H = $(patsubst %.h,$(BUILD_DIR)/%.d,$(SRC_H)) -DEPS_HPP = $(patsubst %.hpp,$(BUILD_DIR)/%.d,$(SRC_HPP)) - -DEPS=$(OBJS:%.o=%.d) +DEPS_C = $(patsubst %.c, $(BUILD_DIR)/%.d, $(SRC_C)) +DEPS_CPP = $(patsubst %.cpp, $(BUILD_DIR)/%.d, $(SRC_CPP)) +DEPS_H = $(patsubst %.h, $(BUILD_DIR)/%.d, $(SRC_H)) +DEPS_HPP = $(patsubst %.hpp, $(BUILD_DIR)/%.d, $(SRC_HPP)) + +#DEPS = $(OBJS: %.o = %.d) +DEPS += $(DEPS_H) +DEPS += $(DEPS_HPP) +DEPS += $(DEPS_C) +DEPS += $(DEPS_CPP) #DEPS+=$(DEPS_H) #DEPS+=$(DEPS_HPP) .DEFAULT: all -.PHONY: all test CPPUTEST CPPUTEST_COMPILE clean +.PHONY: all test CPPUTEST CPPUTEST_COMPILE clean doc all: $(BUILD_DIR) $(BUILD_DIR)/$(OUT_BINARY) +doc: + doxygen $(TOP_DIR)/doc/doxygenconfig + test: CPPUTEST $(BUILD_DIR)/$(OUT_BINARY) $(TEST_DIR) $(BUILD_DIR)/$(OUT_BINARY) -v @@ -88,8 +96,10 @@ CPPUTEST_COMPILE: cd $(CPPUTEST_BUILD) && $(MAKE) $(BUILD_DIR) $(TEST_DIR): + echo $(SRC_C) + echo $(DEPS) $(Q)echo " [mk] $@" - $(Q)mkdir -p $@ + $(Q)mkdir -p $@/src $@/test $@/include clean: $(Q)echo " [rm] $(BUILD_DIR)" @@ -120,4 +130,5 @@ $(BUILD_DIR)/%.d: $(BUILD_DIR) $(TEST_DIR) %.cpp #$(Q)echo -n "$@ $(patsubst %.d,%.o,$@): " >$@ --include $(DEPS) + +#-include $(DEPS) diff --git a/lwrserv/doc/doxygenconfig b/lwrserv/doc/doxygenconfig new file mode 100644 index 0000000..e729215 --- /dev/null +++ b/lwrserv/doc/doxygenconfig @@ -0,0 +1,7 @@ +PROJECT_NAME = lwr_server +OUTPUT_DIRECTORY = html +WARNINGS = YES +INPUT = src +FILE_PATTERNS = *.cc *.h +INCLUDE_PATH = examples src +SEARCHENGINE = YES diff --git a/lwrserv/include/BangBangTrajectory.h b/lwrserv/include/BangBangTrajectory.h index 7d68d20..fec3920 100644 --- a/lwrserv/include/BangBangTrajectory.h +++ b/lwrserv/include/BangBangTrajectory.h @@ -1,7 +1,7 @@ #ifndef _BANGBANGTRAJCETORY_H_ #define _BANGBANGTRAJCETORY_H_ #include "Trajectroy.h" -#include "vec.h" +#include "Vec.h" template class BangBangJointTrajectory : public Trajectory diff --git a/lwrserv/include/LinearTrajectory.h b/lwrserv/include/LinearTrajectory.h index 39b9f28..b608258 100644 --- a/lwrserv/include/LinearTrajectory.h +++ b/lwrserv/include/LinearTrajectory.h @@ -1,9 +1,10 @@ #ifndef _LINEAR_TRAJECTORY_H_ #define _LINEAR_TRAJECTORY_H_ -#include "vec.h" #include "Trajectroy.h" #include "stdlib.h" +#include "Vec.h" +#include "Mat.h" template class LinearJointTrajectory : public Trajectory diff --git a/lwrserv/include/mat.h b/lwrserv/include/Mat.h similarity index 96% rename from lwrserv/include/mat.h rename to lwrserv/include/Mat.h index df3cbde..bd0b345 100644 --- a/lwrserv/include/mat.h +++ b/lwrserv/include/Mat.h @@ -2,7 +2,7 @@ #define _MAT_H_ #include #include -#include "vec.h" +#include "Vec.h" template class Vec; diff --git a/lwrserv/include/robot.h b/lwrserv/include/Robot.h similarity index 54% rename from lwrserv/include/robot.h rename to lwrserv/include/Robot.h index 23f7162..95570f4 100644 --- a/lwrserv/include/robot.h +++ b/lwrserv/include/Robot.h @@ -1,17 +1,22 @@ #ifndef _ROBOT_H_ #define _ROBOT_H_ -#include "vec.h" -#include "mat.h" +#include "Vec.h" +#include "Mat.h" + #include "friComm.h" +#include "friremote.h" +#include "defines.h" +#include "Trajectroy.h" typedef Vec VecTorque; - +#define jointNumber LBR_MNJ +typedef Vec VecJoint; class Robot { - public : - const static int joints = 7; - typedef Vec VecJoint; + public: + + typedef Vec VecJoint; static int getJointCount(void) { @@ -47,49 +52,4 @@ class Robot static MatCarthesian forwardCalc(VecJoint, struct RobotConfig config ); }; - - -#if 0 -Mat4f vecToMat(float vec[12]) -{ - Mat4f result; - for (int i=0; i<3; i++) // ZEILE - { - for (int j=0; j<4; j++) // SPALTE - { - result(i,j) = vec[i*4+j]; - } - } - result(3,3)= 1.0f; - return result; -} - -float* matToVec(Mat4f mat) -{ - float* vec = new float[12]; - for (int j=0;j<3;j++) - { - for (int k=0;k<4;k++) - { - vec[j*4+k] = mat(j,k); - } - } - return vec; -} - -Mat4f getTranslation(double tx, double ty, double tz) -{ - Mat4f transl; - transl(0,0) = 1; - transl(1,1) = 1; - transl(2,2) = 1; - - transl(0,3) = tx; - transl(1,3) = ty; - transl(2,3) = tz; - transl(3,3) = 1; - return transl; -} -#endif - #endif diff --git a/lwrserv/SocketObject.h b/lwrserv/include/SocketObject.h similarity index 100% rename from lwrserv/SocketObject.h rename to lwrserv/include/SocketObject.h diff --git a/lwrserv/StringTool.h b/lwrserv/include/StringTool.h old mode 100755 new mode 100644 similarity index 100% rename from lwrserv/StringTool.h rename to lwrserv/include/StringTool.h diff --git a/lwrserv/include/SvrData.h b/lwrserv/include/SvrData.h index b3dd10d..7889241 100644 --- a/lwrserv/include/SvrData.h +++ b/lwrserv/include/SvrData.h @@ -8,36 +8,33 @@ #include "Singleton.h" #include "Trajectroy.h" -#include "vec.h" #include "friComm.h" #include "friremote.h" -#include "mat.h" -#include "vec.h" -#include "robot.h" - -#define JOINT_NUMBER (LBR_MNJ) +#include "Mat.h" +#include "Vec.h" +#include "Robot.h" class SvrData: public Singleton { private: struct { - Robot::VecJoint jointPos; + VecJoint jointPos; MatCarthesian cartPos; VecTorque forceAndTorque; - float jacobian[FRI_CART_VEC * JOINT_NUMBER]; + float jacobian[FRI_CART_VEC * jointNumber]; } messured; struct { - Robot::VecJoint jointPos; + VecJoint jointPos; MatCarthesian cartPos; } commanded; struct { struct { - Robot::VecJoint velocity; - Robot::VecJoint accelaration; - Robot::VecJoint torque; - Robot::VecJoint range; + VecJoint velocity; + VecJoint accelaration; + VecJoint torque; + VecJoint range; } max; float currentVelocityPercentage; float currentAccelerationPercentage; @@ -47,8 +44,9 @@ private: float sampleTimeMs; bool cancel; bool done; - std::queue< Trajectory* > list; + std::queue< Trajectory* > list; enum TrajectoryType type; + bool potfieldmode; }trajectory; friRemote* friInst; @@ -63,7 +61,7 @@ private: void updateMessurementData( - Robot::VecJoint newJointPos, + VecJoint newJointPos, MatCarthesian newCartPos, VecTorque newForceAndTorque ); @@ -79,32 +77,32 @@ public: void lock(); void unlock(); - Robot::VecJoint getMeasuredJointPos(); + VecJoint getMeasuredJointPos(); MatCarthesian getMeasuredCartPos(); int getMessuredJacobian(float* data, size_t size); VecTorque getMessuredForceTorque(); - Robot::VecJoint getMessuredJointPos(); + VecJoint getMessuredJointPos(); MatCarthesian getMessuredCartPos(); - Robot::VecJoint getCommandedJointPos(); - void setCommandedJointPos(Robot::VecJoint newJointPos); + VecJoint getCommandedJointPos(); + void setCommandedJointPos(VecJoint newJointPos); MatCarthesian getCommandedCartPos(); void setCommandedCartPos(MatCarthesian newCartPos); - Robot::VecJoint getMaxTorque(); - Robot::VecJoint getMaxVelocity(); - Robot::VecJoint getMaxAcceleration(); - Robot::VecJoint getMaxRange(); + VecJoint getMaxTorque(); + VecJoint getMaxVelocity(); + VecJoint getMaxAcceleration(); + VecJoint getMaxRange(); - bool checkJointRange(Robot::VecJoint vectorToCheck); + bool checkJointRange(VecJoint vectorToCheck); - Robot::VecJoint getRobotVelocity(); + VecJoint getRobotVelocity(); float getRobotVelocityPercentage(); int setRobotVelocityPercentage(float newVelocityPercentage); - Robot::VecJoint getRobotAcceleration(); + VecJoint getRobotAcceleration(); float getRobotAccelerationPercentage(); int setRobotAccelerationPercentage(float newAccelerationPercentage); @@ -115,8 +113,8 @@ public: int setSampleTimeMs(float newSampleTime); bool getTrajectoryCancel(); - class Trajectory* popTrajectory(); - int pushTrajectory(class Trajectory* newFrajectory); + class Trajectory* popTrajectory(); + int pushTrajectory(class Trajectory* newFrajectory); bool cancelCurrentTrajectory(); void trajectoryCancelDone(); @@ -125,16 +123,18 @@ public: enum TrajectoryType getTrajectoryType(); void setTrajectoryType(enum TrajectoryType newType); - class Trajectory* createTrajectory(Robot::VecJoint newJointPos); - class Trajectory* createTrajectory(MatCarthesian newJointPos); + class Trajectory* createTrajectory(VecJoint newJointPos); + class Trajectory* createTrajectory(MatCarthesian newJointPos); - template - class Trajectory* calculateTrajectory( - Robot::VecJoint maxJointVelocity, - Robot::VecJoint maxJointAcceleration, - Robot::VecJoint jointStart, - Robot::VecJoint jointEnd + class Trajectory* calculateTrajectory( + VecJoint maxJointVelocity, + VecJoint maxJointAcceleration, + VecJoint jointStart, + VecJoint jointEnd ); - int calculateAndAddNewTrajectory(Robot::VecJoint jointEnd); + int calculateAndAddNewTrajectory(VecJoint jointEnd); + + void setTrajectoryPotfieldMode(bool newstate); + bool getTrajectoryPotfieldMode(); }; #endif diff --git a/lwrserv/SvrHandling.h b/lwrserv/include/SvrHandling.h old mode 100755 new mode 100644 similarity index 100% rename from lwrserv/SvrHandling.h rename to lwrserv/include/SvrHandling.h diff --git a/lwrserv/include/Trajectroy.h b/lwrserv/include/Trajectroy.h index 356c4e2..6c70a7a 100644 --- a/lwrserv/include/Trajectroy.h +++ b/lwrserv/include/Trajectroy.h @@ -1,7 +1,5 @@ #ifndef _TRAJECTORY_H_ #define _TRAJECTORY_H_ -#include "vec.h" -#include "mat.h" #include #include #include @@ -9,6 +7,9 @@ #include #include #include + +#include "Vec.h" +#include "Mat.h" template class Trajectory; // all types of trajectories diff --git a/lwrserv/include/vec.h b/lwrserv/include/Vec.h similarity index 95% rename from lwrserv/include/vec.h rename to lwrserv/include/Vec.h index db212ff..8f73e4f 100644 --- a/lwrserv/include/vec.h +++ b/lwrserv/include/Vec.h @@ -3,7 +3,7 @@ #include #include #include -#include "mat.h" +#include "Mat.h" template class Vec; template class Mat; diff --git a/lwrserv/include/commands.h b/lwrserv/include/commands.h index 3f7ed83..464fa2d 100644 --- a/lwrserv/include/commands.h +++ b/lwrserv/include/commands.h @@ -31,19 +31,12 @@ void setAccel(SocketObject& client, std::string& arg); void startPotFieldMode(SocketObject& client, std::string& arg); //Stopping Potential Field Movement Mode void stopPotFieldMode(SocketObject& client, std::string& arg); -//Setting Target Position HomRowWise for Potential Move -void setPos(SocketObject& client, std::string& arg); -//Setting Target Position as Joints for Potential Move -void setJoints(SocketObject& client, std::string& arg); -//Cartesian Movement -//Move to given POSE position -void moveCartesian(SocketObject& client, std::string& arg); // set a specific trajectory type void setTrajectoryType(SocketObject& client, std::string& arg); void getTrajectoryType(SocketObject& client, std::string& arg); //Quit void quit(SocketObject& client, std::string& arg); -//DEBUGGING -void debug(SocketObject& client, std::string& arg); +// check if we use a kuka +void isKukaLwr(SocketObject& client, std::string& arg); #endif diff --git a/lwrserv/config.h b/lwrserv/include/config.h similarity index 99% rename from lwrserv/config.h rename to lwrserv/include/config.h index 67ace0c..623c90a 100644 --- a/lwrserv/config.h +++ b/lwrserv/include/config.h @@ -1,7 +1,6 @@ /* config.h. Generated from config.h.in by configure. */ /* config.h.in. Generated from configure.ac by autoheader. */ - /* Compiling CppUTest itself */ #define CPPUTEST_COMPILATION 1 diff --git a/lwrserv/include/defines.h b/lwrserv/include/defines.h index f9c9e61..4d74c31 100755 --- a/lwrserv/include/defines.h +++ b/lwrserv/include/defines.h @@ -1,9 +1,11 @@ - #ifndef __DEFINES_H__ #define __DEFINES_H__ -#define M_PI 3.141592654f +#ifndef M_PI + #define M_PI 3.141592654f +#endif +#define REAL_ROBOT false #endif /* __DEFINES_H__ */ diff --git a/lwrserv/friComm.h b/lwrserv/include/friComm.h similarity index 100% rename from lwrserv/friComm.h rename to lwrserv/include/friComm.h diff --git a/lwrserv/friremote.h b/lwrserv/include/friremote.h similarity index 100% rename from lwrserv/friremote.h rename to lwrserv/include/friremote.h diff --git a/lwrserv/friudp.h b/lwrserv/include/friudp.h similarity index 100% rename from lwrserv/friudp.h rename to lwrserv/include/friudp.h diff --git a/lwrserv/include/global.h b/lwrserv/include/global.h deleted file mode 100644 index 4a99459..0000000 --- a/lwrserv/include/global.h +++ /dev/null @@ -1,34 +0,0 @@ -extern bool __SVR_CURRENT_JOB; -extern bool __MSRMSRJNTPOS; -extern bool __MSRCMDJNTPOS; -extern bool __MSRCMDPOSE; -extern bool __SETVELOCITY; -extern bool __SETACCEL; -extern bool __DEBUG; -extern bool __POTFIELDMODE; -extern bool __MSRSTARTPOTFIELD; -extern bool __MSRSTOPPOTFIELD; -extern bool __MSRSETPOS; -extern bool __CARTMOVE; -extern bool __DOUS; -extern bool __DOUS2; - -extern int globi; - -extern float MSRMSRJNTPOS[7]; -extern double MSRMSRCARTPOS[12]; -extern float MSRCMDCARTPOS[12]; -extern double MSRMSRJACOBIAN[42]; -extern double MSRMSRFTTCP[6]; -extern double MSRCMDJNTPOS[7]; -extern double MSRCMDPOSE[3][4]; - -#ifndef SSSS -#define SSSS -//void sleep(DWORD ms); - -//void sleep(DWORD ms) -//{ -// Sleep(ms); -//} -#endif diff --git a/lwrserv/include/lwr4.h b/lwrserv/include/lwr4.h index 1c69296..5a972cd 100644 --- a/lwrserv/include/lwr4.h +++ b/lwrserv/include/lwr4.h @@ -1,10 +1,10 @@ #ifndef _LWR4_H_ #define _LWR4_H_ -#include "vec.h" -#include "mat.h" +#include "Vec.h" +#include "Mat.h" #include "friComm.h" -#include "robot.h" +#include "Robot.h" class LWR4 : public Robot diff --git a/lwrserv/program.cpp b/lwrserv/program.cpp deleted file mode 100644 index 96ee17d..0000000 --- a/lwrserv/program.cpp +++ /dev/null @@ -1,585 +0,0 @@ -#include "sgn.h" -#include -#include -#include -#include - -#include "SvrData.h" -#include "SvrHandling.h" -#include - -#include "mat.h" -#include "vec.h" -#include "robot.h" - -float* abctomat(float a, float b, float c) -{ - Mat4f rx; - float ca = cos(c); - float sa = sin(c); - - rx(0,0) = 1.0f; - rx(1,1) = ca; - rx(1,2) = -sa; - rx(2,1) = sa; - rx(2,2) = ca; - rx(3,3) = 1.0f; - - Mat4f ry; - float cb = cos(b); - float sb = sin(b); - ry(0,0) = cb; - ry(0,2) = sb; - ry(1,1) = 1.0f; - ry(2,0) = -sb; - ry(2,2) = cb; - ry(3,3) = 1.0f; - - Mat4f rz; - float cc = cos(a); - float sc = sin(a); - rz(0,0) = cc; - rz(0,1) = -sc; - rz(1,0) = sc; - rz(1,1) = cc; - rz(2,2) = 1.0f; - rz(3,3) = 1.0f; - - Mat4f result; - Mat4f temp; - temp = rz * ry; - result = temp * rx; - -#ifdef __DEBUG__ - printMat(rx); - printMat(ry); - printMat(rz); - printMat(result); -#endif - - float *res = new float[12]; - //TODO simple converter for mat - for (int j=0;j<3;j++) - { - for (int k=0;k<4;k++) - { - res[j*4+k] = result(j,k); - } - } - return res; -} - -float* mattoabc(float M[12]) -{ - float norm; - float sa; - float ca; - float *abc = new float[3]; - - norm = sqrt((M[0]*M[0])+(M[4]*M[4])); - - if (norm>1e-5) - { - sa = M[4]/norm; - ca = M[0]/norm; - abc[0] = atan2(sa,ca); - } - else - { - sa = 0; - ca = 1; - abc[0] = 0; - } - abc[1] = atan2(-M[8],ca*M[0]+sa*M[4]); - abc[2] = atan2(sa*M[2]-ca*M[6],-sa*M[1]+ca*M[5]); - return abc; -} - -Mat4f vecToMat2(float vec[12]) -{ - Mat4f result; - for (int i=0; i<3; i++) - { - for (int j=0; j<4; j++) - { - result(i,j) = (float)vec[i*4+j]; - } - } - result(3,3)=1.0f; - return result; -} - -float* matToVec2(Mat4f mat) -{ - float* vec = new float[12]; - for (int j=0;j<3;j++) - { - for (int k=0;k<4;k++) - { - vec[j*4+k] = mat(j,k); - } - } - return vec; -} - -float* vectoquat(float vec[12]) -{ - float *quat = new float[4]; - float diag[3]; - int v; int w; int u; - diag[0] = vec[0]; - diag[1] = vec[5]; - diag[2] = vec[10]; - float u_ = *std::max_element(diag,diag+2); - - if (u_==diag[0]) - { - u = 1; - v = 2; - w = 3; - } else if (u_==diag[1]) - { - u = 2; - v = 3; - w = 1; - } else - { - u = 3; - v = 1; - w = 2; - } - float r = sqrt(1+vec[(u-1)*4+(u-1)] - vec[(v-1)*4+(v-1)] - vec[(w-1)*4+(w-1)]); - quat[0] = (vec[(w-1)*4+(v-1)] - vec[(v-1)*4+(w-1)]) / (2*r); - quat[u] = r/2; - quat[v] = (vec[(u-1)*4+(v-1)] + vec[(v-1)*4+(u-1)]) / (2*r); - quat[w] = (vec[(w-1)*4+(u-1)] + vec[(u-1)*4+(w-1)]) / (2*r); - - return quat; -} - -float* quattovec(float quat[4]) -{ - float *vec = new float[12]; - vec[0] = quat[0]*quat[0]+quat[1]*quat[1]-quat[2]*quat[2]-quat[3]*quat[3]; - vec[4] = 2*(quat[1]*quat[2]+quat[0]*quat[3]); - vec[8] = 2*(quat[1]*quat[3]-quat[0]*quat[2]); - - vec[1] = 2*(quat[1]*quat[2]-quat[0]*quat[3]); - vec[5] = quat[0]*quat[0]-quat[1]*quat[1]+quat[2]*quat[2]-quat[3]*quat[3]; - vec[9] = 2*(quat[2]*quat[3]+quat[0]*quat[1]); - - vec[2] = 2*(quat[1]*quat[3]+quat[0]*quat[2]); - vec[6] = 2*(quat[2]*quat[3]-quat[0]*quat[1]); - vec[10] = quat[0]*quat[0]-quat[1]*quat[1]-quat[2]*quat[2]+quat[3]*quat[3]; - - return vec; -} - -void *threadRobotMovement(void *arg) -{ - // unused parameters - (void) arg; - - Trajectory* currentTrajectory = NULL; - enum MovementType currentMovementType = MovementJointBased; - - Robot::VecJoint currentJointPos(0.0f); - MatCarthesian currentCartPos(0.0f,1.0f); - - friRemote* friInst = SvrData::getInstance()->getFriRemote(); - - // get current robot position - int tries = 2; - - for (int i= 0 ; i < tries ; ++i) - { - if (REAL_ROBOT) - SvrData::getInstance()->updateMessurement(); - currentJointPos = SvrData::getInstance()->getMeasuredJointPos(); - - SvrData::getInstance()->setCommandedJointPos(currentJointPos); - - float newJointPosToSend[JOINT_NUMBER] = {0.0f}; - currentJointPos.getData(newJointPosToSend); - if (REAL_ROBOT) - friInst->doPositionControl(newJointPosToSend); - } - std::cout << "init position is " << currentJointPos << "\n"; - - while(true) - { - if (REAL_ROBOT) - SvrData::getInstance()->updateMessurement(); - - bool positionChanged = false; - // check if we have to cancel current trajectory - bool cancel = SvrData::getInstance()->getTrajectoryCancel(); - if (cancel) - { - // free the current trajectory - if ( currentTrajectory != NULL) - { - delete currentTrajectory; - currentTrajectory = NULL; - } - // signal waiting thread the cancellation - SvrData::getInstance()->trajectoryCancelDone(); - goto end; - } - - // check for new trajectory - if (currentTrajectory == NULL) - { - // get next trajectory from svrData - currentTrajectory = SvrData::getInstance()->popTrajectory(); - - // no new trajectory - if(currentTrajectory == NULL) - { - // mark that we reached the end of the trajectory - // stay on current position - goto end; - } - else - { - std::cout <<"new Trajectory with " << currentTrajectory->getSteps()<<"\n"; - } - } - - // get next commanded trajectory type - if (currentTrajectory != NULL) - { - switch (currentTrajectory->getMovementType()) - { - case MovementCartBased: - { - currentCartPos = currentTrajectory->getNextCartPos(); - positionChanged = true; - currentMovementType = MovementCartBased; - } - break; - case MovementJointBased: - { - currentJointPos = currentTrajectory->getNextJointPos(); - positionChanged = true; - currentMovementType = MovementJointBased; - } - break; - default: - { - // invalid trajectory skip it - delete currentTrajectory; - currentTrajectory = NULL; - goto end; - } - break; - } - } - -end: - switch (currentMovementType) - { - case MovementJointBased: - { - // send the new joint values - float newJointPosToSend[JOINT_NUMBER] = {0.0f}; - Robot::VecJoint newJointPosToSendRad = currentJointPos*(1./180*M_PI); - newJointPosToSendRad.getData(newJointPosToSend); - if (REAL_ROBOT) - friInst->doPositionControl(newJointPosToSend); - } - break; - case MovementCartBased: - { - float Stiff[6] = {1000.0, 1000.0, 1000.0, 10.0, 10.0, 10.0}; - float Damp[6] = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7}; - float newCartPosToSend[12] = {0.0f}; - currentCartPos.getData(newCartPosToSend); - if (REAL_ROBOT) - friInst->doCartesianImpedanceControl(newCartPosToSend,Stiff, Damp, NULL, NULL, true); - } - break; - } - - // start timer for the last sent msg - // TODO - - // check if current trajectory is over - if(currentTrajectory != NULL && currentTrajectory->getRemainingSteps() == 0) - { - std::cout << " Trajectory finished \n "; - - static int trajcetorycount = 0; - trajcetorycount +=1; - - currentTrajectory->saveToFile(std::string("trajectory/a.csv")); - // invalid trajectory skip it - delete currentTrajectory; - currentTrajectory = NULL; - } - } -} - -#if 0 -void *threadFriDataExchange(void *arg) -{ - // unused - (void) arg; - - friRemote* friInst = SvrData::getInstance()->getFriRemote(); - - while (1) - { - //####################################### - // Communication loop - - // update current joint positions - SvrData::getInstance()->updateMessurement(); - - // get current joint position - Robot::VecJoint currentJointPos = SvrData::getInstance()->getMessuredJointPos(); - - MatCarthesian currentCartPost = SvrData::getInstance()->getMessuredCartPos(); - - // get current force and torque - Robot::VecJoint currentForceAndTorque = SvrData::getInstance()->getMessuredForceTorque(); - - // get current jacobian - // TODO use svr data - float* friJacobian = friInst->getJacobian(); - if ( friJacobian == NULL) - { - fprintf(stderr,"Failed: could not get jacobian\n"); - break; - } - for (int i = 0; i < FRI_CART_VEC*LBR_MNJ ; i++) - { - MSRMSRJACOBIAN[i] = friJacobian[i]; - } - - //######################################################### - // PTP Joint Movement - if (__MSRCMDJNTPOS) - { - Vec maxJointLocalVelocity; - Vec maxJointLocalAcceleration; - Vec delta; - Vec deltaAbs; - Vec dMaxSpeed; - Vec lMaxSpeed; - Vec dGesamt; - float sampleTime = SvrData::getInstance()->getSampleTimeMs(); - - // get current robot constraints - Vec maxVelocityJoint = SvrData::getInstance()->getMaxVelocity(); - Vec maxAccelarationJoint = SvrData::getInstance()->getMaxAcceleration(); - Vec messuredJointPos = SvrData::getInstance()->getMessuredJointPos(); - Vec commandedJointPos = SvrData::getInstance()->getCommandedJointPos(); - - float velocity = SvrData::getInstance()->getRobotVelocity(); - float accelaration = SvrData::getInstance()->getRobotAcceleration(); - - // calculate delta positions of movement - delta = commandedJointPos - messuredJointPos; - deltaAbs = delta.abs(); - maxJointLocalVelocity = maxVelocityJoint * sampleTime * (velocity/100.0f); - maxJointLocalAcceleration = maxAccelarationJoint * sampleTime * (accelaration/100.0f); - - // calculate number of movement steps - dMaxSpeed = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration); - lMaxSpeed = dMaxSpeed.cellmultiply(dMaxSpeed).cellmultiply(maxJointLocalAcceleration) * 0.5f; - - for (int j=0; j deltaAbs(j)/(double)2.0) - { - dGesamt(j) = sqrt(deltaAbs(j) / maxJointLocalAcceleration (j))*2.0; - } else - { - dGesamt(j) = dMaxSpeed(j)*2 + (deltaAbs(j)-lMaxSpeed(j)*2.0)/maxJointLocalVelocity(j); - } - } - int maxSteps = ceil(dGesamt.max()); - - // there are atleast 2 steps otherwise it is no trajectory - if (maxSteps == 0 || maxSteps == 1) - goto end; - - for (int j=0; j currentInk; - Vec currentInkLast; - Vec currentDist; - Vec currentDistLast; - - // bang bang - for (int i=0;i maxSteps-dMaxSpeed(j)) - { - currentInk(j) = max(currentInkLast(j)-maxJointLocalAcceleration(j),0.0f); - }else - { - currentInk(j) = currentInkLast(j); - } - currentDist(j) = currentDistLast(j) + sgn(delta(j))*currentInk(j); - currentInkLast(j) = currentInk(j); - currentDistLast(j) = currentDist(j); - MSRMSRJNTPOS[j] += sgn(delta(j))*currentInk(j)*(1./180*M_PI); - } - // set new position - friInst->doPositionControl(MSRMSRJNTPOS); - } - } -end: - // mark state to be done - __MSRCMDJNTPOS = false; - - } else if (__CARTMOVE) - { - //########################################################## - //Cartesian Movement - - const float Stiff[6] = {1000.0, 1000.0, 1000.0, 150.0, 150.0, 150.0}; - const float Damp[6] = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7}; - float th = 0.000; - float deltaCart[12]; - float Pos[12]; - float dist; - float rot; - float sum; - float b; - float c; - float t_fact = 1; - float t_outerT = 0.007; - float t_innerT = 0.000; - float t_outerR = 0.007; - float t_innerR = 0.000; - - while (true) - { - for ( int i =0; i < FRI_CART_FRM_DIM; i++) - { - Pos[i] = friInst->getMsrCartPosition()[i]; - MSRMSRCARTPOS[i] = friInst->getMsrCartPosition()[i]; - deltaCart[i] = MSRCMDCARTPOS[i]-Pos[i]; - } - sum = 0; - for (int i=0; i<3 ; i++) - { - sum = sum + (deltaCart[(i*4)+3]*deltaCart[(i*4)+3]); - } - dist = sqrt(sum); - sum = 0; - for (int i=0;i<12;i++) - { - if (i != 3 && i != 7 && i != 11) - { - sum = sum + deltaCart[i]*deltaCart[i]; - } - } - rot = sqrt(sum); - b = t_fact*max((float)0.0,(float)min((float)1.0,(t_outerT-dist+(t_innerT-t_outerT))*(1/(t_innerT-t_outerT)))); - c = t_fact*max((float)0.0,(float)min((float)1.0,(float)(t_outerR-rot+(t_innerR-t_outerR))*(float)(1/(t_innerR-t_outerR)))); - - if (dist < th) - { - for (int i=0; i<3; i++) - { - deltaCart[(i*4)+3] = 0; - } - }else - { - for (int i=0; i<3; i++) - { - deltaCart[(i*4)+3] = (deltaCart[(i*4)+3] / dist)*b; - } - } - - // check offset to point - if (rot < th) - { - for (int i=0;i<12;i++) - { - if (i != 3 && i != 7 && i != 11) - { - deltaCart[i] = 0; - } - } - }else - { - for (int i=0;i<12;i++) - { - if (i != 3 && i != 7 && i != 11) - { - deltaCart[i] = (deltaCart[i] / rot)*c; - } - } - } - for (int i=0;i<12;i++) - { - Pos[i]+= deltaCart[i]*0.02; - } - friInst->doCartesianImpedanceControl(Pos, Stiff,Damp,NULL,NULL, true); - - } - } - //float Stiff[6] = {1000.0, 1000.0, 1000.0, 10.0, 10.0, 10.0}; - //float Damp[6] = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7}; - //float Test[12] = {MSRMSRCARTPOS[0],MSRMSRCARTPOS[1],MSRMSRCARTPOS[2],MSRMSRCARTPOS[3],MSRMSRCARTPOS[4],MSRMSRCARTPOS[5],MSRMSRCARTPOS[6],MSRMSRCARTPOS[7],MSRMSRCARTPOS[8],MSRMSRCARTPOS[9],MSRMSRCARTPOS[10],MSRMSRCARTPOS[11]}; - //friInst->doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true); - - // innerhlab 5ms - friInst->doPositionControl(MSRMSRJNTPOS); - - //i = 0; - //} - //friInst->doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true); - } - return NULL; -} -#endif - -int main() -{ - int err = 0; - //Setting pthread for FRI interface - pthread_t fri_t; - //Start fri_thread - err = pthread_create(&fri_t,NULL,&threadRobotMovement,NULL); - if (err > 0 ) - { - std::cerr << "Failed: could not create thread\n" << std::endl; - return err; - } - - //Start client handling - SvrHandling *svr = new SvrHandling(); - if (svr == NULL) - { - std::cerr << "Failed: could not create server \n" << std::endl; - return -ENOMEM; - } - svr->run(); - - return 0; -} diff --git a/lwrserv/SocketObject.cpp b/lwrserv/src/SocketObject.cpp similarity index 100% rename from lwrserv/SocketObject.cpp rename to lwrserv/src/SocketObject.cpp diff --git a/lwrserv/StringTool.cpp b/lwrserv/src/StringTool.cpp old mode 100755 new mode 100644 similarity index 100% rename from lwrserv/StringTool.cpp rename to lwrserv/src/StringTool.cpp diff --git a/lwrserv/SvrData.cpp b/lwrserv/src/SvrData.cpp similarity index 80% rename from lwrserv/SvrData.cpp rename to lwrserv/src/SvrData.cpp index b59d682..97a2a9e 100644 --- a/lwrserv/SvrData.cpp +++ b/lwrserv/src/SvrData.cpp @@ -8,16 +8,15 @@ #include "friremote.h" #include -#include "mat.h" -#include "vec.h" -#include "robot.h" +#include "Mat.h" +#include "Vec.h" +#include "Robot.h" bool SvrData::instanceFlag = false; SvrData* SvrData::single = 0; SvrData::SvrData(void) { - // these are the default values for the robot double maxVelJnt[] = {110.0,110.0,128.0,128.0,204.0,184.0,184.0}; double maxAccJnt[] = {1.0,1.0,1.0,1.0,2.0,1.0,1.0}; @@ -27,7 +26,7 @@ SvrData::SvrData(void) robot.currentVelocityPercentage = 10; robot.currentAccelerationPercentage = 10; - for (unsigned int i = 0; i < JOINT_NUMBER; ++i) + for (unsigned int i = 0; i < jointNumber; ++i) { robot.max.velocity(i) = maxVelJnt[i]; robot.max.accelaration(i) = maxAccJnt[i]; @@ -71,7 +70,7 @@ friRemote* SvrData::getFriRemote() } void SvrData::updateMessurementData( - Robot::VecJoint newJointPos, + VecJoint newJointPos, MatCarthesian newCartPos, VecTorque newForceAndTorque ) @@ -85,7 +84,7 @@ void SvrData::updateMessurementData( void SvrData::updateMessurement() { - Robot::VecJoint newJointPos(0.0f); + VecJoint newJointPos(0.0f); MatCarthesian newCartPos(0.0f,1.0f); VecTorque newForceAndTorque(0.0f); @@ -93,8 +92,8 @@ void SvrData::updateMessurement() /// update current joint positions friInst->getState(); FRI_STATE state = friInst->getState(); - Robot::VecJoint newJointPosComanded(friInst->getMsrCmdJntPosition()); - Robot::VecJoint newJointPosOffset(friInst->getMsrCmdJntPositionOffset()); + VecJoint newJointPosComanded(friInst->getMsrCmdJntPosition()); + VecJoint newJointPosOffset(friInst->getMsrCmdJntPositionOffset()); newJointPos = newJointPosComanded + newJointPosOffset; newJointPos = newJointPos * ( 180.0f / M_PI); @@ -119,9 +118,9 @@ void SvrData::unlock() pthread_mutex_unlock(&dataLock); } -Robot::VecJoint SvrData::getMessuredJointPos() +VecJoint SvrData::getMessuredJointPos() { - Robot::VecJoint buf; + VecJoint buf; pthread_mutex_lock(&dataLock); buf = messured.jointPos; pthread_mutex_unlock(&dataLock); @@ -158,15 +157,15 @@ int SvrData::getMessuredJacobian(float* data, size_t size) return 0; } -Robot::VecJoint SvrData::getCommandedJointPos() +VecJoint SvrData::getCommandedJointPos() { - Robot::VecJoint buff; + VecJoint buff; pthread_mutex_lock(&dataLock); buff = commanded.jointPos; pthread_mutex_unlock(&dataLock); return buff; } -void SvrData::setCommandedJointPos(Robot::VecJoint newJointPos) +void SvrData::setCommandedJointPos(VecJoint newJointPos) { pthread_mutex_lock(&dataLock); commanded.jointPos = newJointPos; @@ -188,41 +187,41 @@ void SvrData::setCommandedCartPos(MatCarthesian newCartPos) } -Robot::VecJoint SvrData::getMaxTorque() +VecJoint SvrData::getMaxTorque() { - Robot::VecJoint buff; + VecJoint buff; pthread_mutex_lock(&dataLock); buff = robot.max.torque; pthread_mutex_unlock(&dataLock); return buff; } -Robot::VecJoint SvrData::getMaxAcceleration() +VecJoint SvrData::getMaxAcceleration() { - Robot::VecJoint retval(0.0f); + VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); retval = robot.max.accelaration; pthread_mutex_unlock(&dataLock); return retval; } -Robot::VecJoint SvrData::getMaxVelocity() +VecJoint SvrData::getMaxVelocity() { - Robot::VecJoint retval(0.0f); + VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); retval = robot.max.velocity; pthread_mutex_unlock(&dataLock); return retval; } -Robot::VecJoint SvrData::getMaxRange() +VecJoint SvrData::getMaxRange() { - Robot::VecJoint retval(0.0f); + VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); retval = robot.max.range; pthread_mutex_unlock(&dataLock); return retval; } -bool SvrData::checkJointRange(Robot::VecJoint vectorToCheck) +bool SvrData::checkJointRange(VecJoint vectorToCheck) { // save temporal the max range for short time lock pthread_mutex_lock(&dataLock); @@ -231,7 +230,7 @@ bool SvrData::checkJointRange(Robot::VecJoint vectorToCheck) pthread_mutex_unlock(&dataLock); // check if the value is in plus or minus range - for (int i = 0; i abs(maxJointRange(i))) { @@ -258,15 +257,13 @@ int SvrData::setRobotVelocityPercentage(float newVelocityPercentage) pthread_mutex_unlock(&dataLock); return retval; } -Robot::VecJoint SvrData::getRobotVelocity() +VecJoint SvrData::getRobotVelocity() { float percent = 0.0f; - Robot::VecJoint retval(0.0f); + VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); percent = robot.currentVelocityPercentage / 100.0f; - std::cout << "percent " << percent << " velo " << retval; - std::cout << robot.max.velocity << "max velo \n"; retval = robot.max.velocity * percent; pthread_mutex_unlock(&dataLock); return retval; @@ -295,10 +292,10 @@ int SvrData::setRobotAccelerationPercentage(float newAccelerationPercentage) pthread_mutex_unlock(&dataLock); return retval; } -Robot::VecJoint SvrData::getRobotAcceleration() +VecJoint SvrData::getRobotAcceleration() { float percent = 0; - Robot::VecJoint retval(0.0f); + VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); percent = robot.currentAccelerationPercentage / 100.0f; @@ -320,7 +317,7 @@ unsigned int SvrData::getJoints() unsigned int retval = 0; pthread_mutex_lock(&dataLock); - retval = JOINT_NUMBER; + retval = jointNumber; pthread_mutex_unlock(&dataLock); return retval; } @@ -349,9 +346,9 @@ int SvrData::setSampleTimeMs(float newSampleTimeMs) return retval; } -class Trajectory* SvrData::popTrajectory() +class Trajectory* SvrData::popTrajectory() { - class Trajectory* retval = NULL; + class Trajectory* retval = NULL; pthread_mutex_lock(&dataLock); if (!trajectory.list.empty() ) { @@ -366,7 +363,7 @@ class Trajectory* SvrData::popTrajectory() pthread_mutex_unlock(&dataLock); return retval; } -int SvrData::pushTrajectory(class Trajectory* newTrajectory) +int SvrData::pushTrajectory(class Trajectory* newTrajectory) { if (newTrajectory == NULL) return -1; @@ -395,7 +392,7 @@ bool SvrData::cancelCurrentTrajectory() // clear trajectory list while(!trajectory.list.empty()) { - class Trajectory* currentTrajectory = trajectory.list.front(); + class Trajectory* currentTrajectory = trajectory.list.front(); if(currentTrajectory != NULL) delete currentTrajectory; trajectory.list.pop(); @@ -448,32 +445,54 @@ void SvrData::setTrajectoryType(enum TrajectoryType newType) pthread_mutex_unlock(&dataLock); } -class Trajectory* SvrData::createTrajectory(Robot::VecJoint newJointPos) +class Trajectory* SvrData::createTrajectory(VecJoint newJointPos) { //set new target for next trajectory - Robot::VecJoint prevJointPos = getCommandedJointPos(); - class Trajectory* newTrajectory; + VecJoint prevJointPos = getCommandedJointPos(); + class Trajectory* newTrajectory; float sampleTimeMs = getSampleTimeMs(); - Robot::VecJoint maxJointVelocity = getRobotVelocity(); - Robot::VecJoint maxJointAcceleration = getRobotAcceleration(); + VecJoint maxJointVelocity = getRobotVelocity(); + VecJoint maxJointAcceleration = getRobotAcceleration(); enum TrajectoryType type = getTrajectoryType(); //newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos); - newTrajectory = Trajectory::GetTrajectory(type,sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos); + newTrajectory = Trajectory::GetTrajectory(type,sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos); return newTrajectory; } -class Trajectory* SvrData::createTrajectory(MatCarthesian newJointPos) +class Trajectory* SvrData::createTrajectory(MatCarthesian newJointPos) { } -Robot::VecJoint SvrData::getMeasuredJointPos() +VecJoint SvrData::getMeasuredJointPos() { - Robot::VecJoint retval(0.0f); + VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); retval = messured.jointPos; pthread_mutex_unlock(&dataLock); return retval; } -MatCarthesian getMeasuredCartPos(); +MatCarthesian SvrData::getMeasuredCartPos() +{ + MatCarthesian retval(0.0f,1.0f); + pthread_mutex_lock(&dataLock); + retval = messured.cartPos; + pthread_mutex_unlock(&dataLock); + return retval; +} + +void SvrData::setTrajectoryPotfieldMode(bool newstate) +{ + pthread_mutex_lock(&dataLock); + trajectory.potfieldmode = newstate; + pthread_mutex_unlock(&dataLock); +} +bool SvrData::getTrajectoryPotfieldMode() +{ + bool retval = false; + pthread_mutex_lock(&dataLock); + retval = trajectory.potfieldmode; + pthread_mutex_unlock(&dataLock); + return retval; +} diff --git a/lwrserv/SvrHandling.cpp b/lwrserv/src/SvrHandling.cpp old mode 100755 new mode 100644 similarity index 88% rename from lwrserv/SvrHandling.cpp rename to lwrserv/src/SvrHandling.cpp index d8dad02..23ac9e0 --- a/lwrserv/SvrHandling.cpp +++ b/lwrserv/src/SvrHandling.cpp @@ -1,16 +1,16 @@ #include -#include "mat.h" -#include "StringTool.h" #include #include #include +#include +#include "Mat.h" +#include "StringTool.h" #include "SvrHandling.h" #include "SvrData.h" #include "commands.h" #include "Trajectroy.h" #include "LinearTrajectory.h" -#include void printUsage(SocketObject& client, std::string& arg) { @@ -76,16 +76,6 @@ SvrHandling::SvrHandling() commands[i].processCommand = &stopPotFieldMode; commands[i].printHelp = NULL; i+=1; - commands[i].aberration = "SP"; - commands[i].longVersion = "SetPos"; - commands[i].processCommand = &setPos; - commands[i].printHelp = NULL; - i+=1; - commands[i].aberration = "SJ"; - commands[i].longVersion = "SetJoints"; - commands[i].processCommand = &setJoints; - commands[i].printHelp = NULL; - i+=1; commands[i].aberration = "Q"; commands[i].longVersion = "Quit"; commands[i].processCommand = &quit; @@ -98,12 +88,7 @@ SvrHandling::SvrHandling() i+=1; commands[i].aberration = "ISKUKA"; commands[i].longVersion = "IsKukaLWR"; - commands[i].processCommand = NULL; - commands[i].printHelp = NULL; - i+=1; - commands[i].aberration = "MC"; - commands[i].longVersion = "MoveCartesian"; - commands[i].processCommand = &moveCartesian; + commands[i].processCommand = &isKukaLwr; commands[i].printHelp = NULL; i+=1; commands[i].aberration = "GT"; diff --git a/lwrserv/commands.cpp b/lwrserv/src/commands.cpp similarity index 52% rename from lwrserv/commands.cpp rename to lwrserv/src/commands.cpp index e18b1e1..2152240 100644 --- a/lwrserv/commands.cpp +++ b/lwrserv/src/commands.cpp @@ -1,23 +1,63 @@ +/** + * @file + * @author John Doe + * @version 1.0 + * + * @section LICENSE + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License as + * published by the Free Software Foundation; either version 2 of + * the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details at + * https://www.gnu.org/copyleft/gpl.html + * + * @section DESCRIPTION + * + * The time class represents a moment of time. + */ +#include +#include +#include +#include +#include +#include + #include "commands.h" -#include "robot.h" +#include "Robot.h" #include "lwr4.h" #include "SocketObject.h" #include "Trajectroy.h" #include "SvrHandling.h" #include "SvrData.h" -#include -#include -#include "mat.h" -#include "vec.h" +#include "Mat.h" +#include "Vec.h" #include "StringTool.h" -#include -#include -#include -#include +void moveRobotTo(VecJoint newJointPos) +{ + Trajectory* newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos); + + // add trajectory to queue for sender thread + SvrData::getInstance()->pushTrajectory(newTrajectory); + SvrData::getInstance()->setCommandedJointPos(newJointPos); + + //Wait to end of movement + float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs(); + while (true) + { + if (SvrData::getInstance()->getTrajectoryDone() == true) + break; + boost::this_thread::sleep( boost::posix_time::milliseconds(sampleTimeMs) ); + } +} void getPositionJoints(SocketObject& client, std::string& arg) { // unused @@ -27,10 +67,10 @@ void getPositionJoints(SocketObject& client, std::string& arg) std::stringstream ss (std::stringstream::in | std::stringstream::out); // get current joint positions from database - Robot::VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos(); + VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos(); // assemble command feadback - for (int i=0; i< JOINT_NUMBER; i++) + for (int i=0; i< jointNumber; i++) { ss.str(""); ss << (jointPos(i)*180/M_PI); @@ -93,12 +133,12 @@ void movePTPJoints(SocketObject& client, std::string& arg) std::stringstream ss2f; // convert string to joint vector - Robot::VecJoint newJointPos(0.0f); + VecJoint newJointPos(0.0f); int i = 0; while (ss >> buf) { // to many joint values - if (i>=JOINT_NUMBER) + if (i>=jointNumber) { client.Send(SVR_INVALID_ARGUMENT_COUNT); return; @@ -111,13 +151,12 @@ void movePTPJoints(SocketObject& client, std::string& arg) } // to less joint values - if (i!=JOINT_NUMBER) + if (i!=jointNumber) { client.Send(SVR_INVALID_ARGUMENT_COUNT); return ; } - //check for joint range if (!SvrData::getInstance()->checkJointRange(newJointPos)) { @@ -126,21 +165,7 @@ void movePTPJoints(SocketObject& client, std::string& arg) return; } - Trajectory* newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos); - - // add trajectory to queue for sender thread - SvrData::getInstance()->pushTrajectory(newTrajectory); - - SvrData::getInstance()->setCommandedJointPos(newJointPos); - - //Wait to end of movement - float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs(); - while (true) - { - if (SvrData::getInstance()->getTrajectoryDone() == true) - break; - boost::this_thread::sleep( boost::posix_time::milliseconds(sampleTimeMs) ); - } + moveRobotTo(newJointPos); client.Send(SVR_TRUE_RSP); } @@ -193,39 +218,17 @@ void moveHomRowWiseStatus(SocketObject& client, std::string& arg) configurationParameter.j1os = temp[14] == 1.0f; //Backward Calculation - Robot::VecJoint newJointPos = LWR4::backwardCalc(newCartPos, configurationParameter); + VecJoint newJointPos = LWR4::backwardCalc(newCartPos, configurationParameter); - - //Check for Joint Range + //check for joint range if (!SvrData::getInstance()->checkJointRange(newJointPos)) { + std::string out = "Error: Joints out of Range!"; client.Send(SVR_OUT_OF_RANGE); return; } - // calculate trajectory - Robot::VecJoint prevJointPos = SvrData::getInstance()->getCommandedJointPos(); - float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs(); - Robot::VecJoint maxJointVelocity = SvrData::getInstance()->getRobotVelocity(); - Robot::VecJoint maxJointAcceleration = SvrData::getInstance()->getRobotAcceleration(); - std::cout << "max velo is " << maxJointVelocity ; - - class Trajectory* newTrajectory = new LinearJointTrajectory(sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos); - - // set new commanded position for next trajectory - SvrData::getInstance()->setCommandedJointPos(newJointPos); - - // add new trajectory to position - SvrData::getInstance()->pushTrajectory(newTrajectory); - - //Wait to end of movement - while (true) - { - if (SvrData::getInstance()->getTrajectoryDone() == true) - break; - boost::this_thread::sleep( boost::posix_time::milliseconds(sampleTimeMs) ); - } - + moveRobotTo(newJointPos); // send a positive client feedback client.Send(SVR_TRUE_RSP); } @@ -324,23 +327,13 @@ void startPotFieldMode(SocketObject& client, std::string& arg) { // unused (void) arg; -#if 0 - __MSRSTARTPOTFIELD = false; - __MSRSTOPPOTFIELD = false; + //Set current Joint Vals - for (int i=0; igetMessuredJointPos(); + SvrData::getInstance()->setTrajectoryPotfieldMode(true); + + moveRobotTo(currJoints); + // send a positive client feedback client.Send(SVR_TRUE_RSP); } @@ -348,203 +341,7 @@ void stopPotFieldMode(SocketObject& client, std::string& arg) { // unused (void) arg; -#if 0 - __POTFIELDMODE = false; - - while (1) - { - if (!__MSRSTOPPOTFIELD) - { - break; - } - } -#endif - client.Send(SVR_TRUE_RSP); -} - -void setPos(SocketObject& client, std::string& arg) -{ - std::string buf; - double temp[15]; - std::stringstream ss(arg); - std::stringstream ss2f; - int argc = 0; - - // convert position arguments from string to float - while (ss >> buf) - { - // only 15 parameters are necessary - if (argc>=15) - { - client.Send(SVR_FALSE_RSP); - return; - } - - ss2f.flush(); - ss2f.clear(); - ss2f << buf; - ss2f >> temp[argc]; - argc++; - } - - // check for exactly 15 parameters - if (argc!=15) - { - client.Send(SVR_FALSE_RSP); - return ; - } - - - // the position is saved in the first parameter part - MatCarthesian newCartPos(0.0f,1.0f); - for (int i=0 ;i<3; i++) - { - for (int j=0;j<4;j++) - { - newCartPos(i,j)=temp[i*4+j]; - } - } - - // extract elbow flip and hand parameter - struct Robot::RobotConfig configurationParameter; - configurationParameter.elbow = temp[12] == 1.0f; - configurationParameter.flip = temp[13] == 1.0f; - configurationParameter.j1os = temp[14] == 1.0f; - - //Backward Calculation - Robot::VecJoint newJointPos = LWR4::backwardCalc(newCartPos, configurationParameter); - - //Check for a valid joint range - if (!SvrData::getInstance()->checkJointRange(newJointPos)) - { - client.Send(SVR_OUT_OF_RANGE); - return; - } - - // calculate new trajectory for next position - - // set new commanded Position for next Trajectory - SvrData::getInstance()->setCommandedJointPos(newJointPos); - - //Wait to end of movement - - // send positive client feedback - client.Send(SVR_TRUE_RSP); -} - -void setJoints(SocketObject& client, std::string& arg) -{ - std::string buf; - Robot::VecJoint newJointPos(0.0f); - std::stringstream ss(arg); - std::stringstream ss2f; - int argc = 0; - - // convert to Joint vector - while (ss >> buf) - { - // to many input arguments or Joints - if (argc>=JOINT_NUMBER) - { - client.Send(SVR_INVALID_ARGUMENT_COUNT); - return; - } - - // convert string to float - ss2f.flush(); - ss2f.clear(); - ss2f << buf; - ss2f >> newJointPos(argc); - argc++; - } - - // to less joint values - if (argc != JOINT_NUMBER) - { - client.Send(SVR_INVALID_ARGUMENT_COUNT); - return; - } - - //Check for Joint Range - if (!SvrData::getInstance()->checkJointRange(newJointPos)) - { - client.Send(SVR_OUT_OF_RANGE); - return; - } - - // calculate trajectory from last commanded joint position - Robot::VecJoint currentComandedJoint = SvrData::getInstance()->getCommandedJointPos(); - - // set new trajectory - - // Set new target Position and also calculate Cartesian position - SvrData::getInstance()->setCommandedJointPos(newJointPos); - // TODO calculate and calculate position in Cartesian coordinates - - // wait till position is reached - - // return positive response to client - client.Send(SVR_TRUE_RSP); -} - -void moveCartesian(SocketObject& client, std::string& arg) -{ -#if 0 - std::string buf; - float temp[15]; - std::stringstream ss(arg); - std::stringstream ss2f; - int argc = 0; - - // convert Cartesian coordinates from string into float - while (ss >> buf) - { - // only need maximum 16 coordinates for a homogeneous matrix - if (argc>=15) - { - client.Send(SVR_FALSE_RSP); - return; - } - ss2f.flush(); - ss2f.clear(); - ss2f << buf; - ss2f >> temp[argc]; - argc++; - } - - // check for exactly 15 coordinates - if (argc != 15) - { - client.Send(SVR_FALSE_RSP); - return ; - } - - float configurationParameter[3]; - for (int i=0;i<3;i++) - { - configurationParameter[i] = temp[12+i]; - } - - //Calculating end-effector position for target "TRobot" - Mat4f Tsurface; - Tsurface = vecToMat(temp); - Mat4f TRobot; - TRobot = Tsurface; - - //Calculating end-effector position for approach position "ApRobot" - Mat4f ApRobot; - Mat4f ApSurface; - Mat4f TransZ; - // 100mm approach position - TransZ = getTranslation(0,0,-0.1); - ApSurface = Tsurface * TransZ; - ApRobot = ApSurface ; - float* tempVec = NULL; - tempVec = matToVec(TRobot); - //tempVec = matToVec(ApRobot); - -#endif - // send a positive client feedback + SvrData::getInstance()->setTrajectoryPotfieldMode(false); client.Send(SVR_TRUE_RSP); } @@ -564,31 +361,37 @@ void setTrajectoryType(SocketObject& client, std::string& arg) newType = toEnum(arg); SvrData::getInstance()->setTrajectoryType(newType); - std::cout << "new trajectory type : "<< toString(newType) << "\n"; - client.Send(SVR_TRUE_RSP); + client.Send("new trajectory: " + toString(newType)); + if (toString(newType) == arg) + client.Send(SVR_TRUE_RSP); + else + client.Send(SVR_FALSE_RSP); } + void getTrajectoryType(SocketObject& client, std::string& arg) { + //unused parameter + (void) arg; + enum TrajectoryType currType = SvrData::getInstance()->getTrajectoryType(); - std::cout << "curr trajectory type : "<< toString(currType) << "\n"; client.Send("current trajectory: " + toString(currType)); + client.Send(SVR_TRUE_RSP); } -void debug(SocketObject& client, std::string& arg) +/** + * clients can check if the robot is of Type Kuka LWR + * @param client connection to client which will recive the response + * @param arg aguments which are unused in this case + * + * @author Philipp Schoenberger + * @date + */ +void isKukaLwr(SocketObject& client, std::string& arg) { + //unused parameter + (void) arg; - //check = true; - //float temp[7]; - //while (1){ - // friInst.setToKRLInt(15,1); - // friInst.doSendData(); - // //client.Send(friInst.getFrmKRLInt(2)); - // for ( int i= 0; i < LBR_MNJ; i++) - //{ - // temp[i]=friInst.getMsrMsrJntPosition()[i]; - //} - //friInst.doPositionControl(temp); - //} + client.Send(SVR_TRUE_RSP); } diff --git a/lwrserv/friremote.cpp b/lwrserv/src/friremote.cpp similarity index 100% rename from lwrserv/friremote.cpp rename to lwrserv/src/friremote.cpp diff --git a/lwrserv/friudp.cpp b/lwrserv/src/friudp.cpp similarity index 100% rename from lwrserv/friudp.cpp rename to lwrserv/src/friudp.cpp diff --git a/lwrserv/src/main.cpp b/lwrserv/src/main.cpp new file mode 100644 index 0000000..be1f29a --- /dev/null +++ b/lwrserv/src/main.cpp @@ -0,0 +1,183 @@ +#include "sgn.h" +#include +#include +#include +#include + +#include "Robot.h" +#include "SvrData.h" +#include "SvrHandling.h" +#include + +#include "Mat.h" +#include "Vec.h" + +void *threadRobotMovement(void *arg) +{ + // unused parameters + (void) arg; + + Trajectory* currentTrajectory = NULL; + enum MovementType currentMovementType = MovementJointBased; + + VecJoint currentJointPos(0.0f); + MatCarthesian currentCartPos(0.0f,1.0f); + + friRemote* friInst = SvrData::getInstance()->getFriRemote(); + + // get current robot position + int tries = 2; + + for (int i= 0 ; i < tries ; ++i) + { + if (REAL_ROBOT) + SvrData::getInstance()->updateMessurement(); + currentJointPos = SvrData::getInstance()->getMeasuredJointPos(); + + SvrData::getInstance()->setCommandedJointPos(currentJointPos); + + float newJointPosToSend[jointNumber] = {0.0f}; + currentJointPos.getData(newJointPosToSend); + if (REAL_ROBOT) + friInst->doPositionControl(newJointPosToSend); + } + std::cout << "init position is " << currentJointPos << "\n"; + + while(true) + { + if (REAL_ROBOT) + SvrData::getInstance()->updateMessurement(); + + bool positionChanged = false; + // check if we have to cancel current trajectory + bool cancel = SvrData::getInstance()->getTrajectoryCancel(); + if (cancel) + { + // free the current trajectory + if ( currentTrajectory != NULL) + { + delete currentTrajectory; + currentTrajectory = NULL; + } + // signal waiting thread the cancellation + SvrData::getInstance()->trajectoryCancelDone(); + goto end; + } + + // check for new trajectory + if (currentTrajectory == NULL) + { + // get next trajectory from svrData + currentTrajectory = SvrData::getInstance()->popTrajectory(); + + // no new trajectory + if(currentTrajectory == NULL) + { + // mark that we reached the end of the trajectory + // stay on current position + goto end; + } + else + { + std::cout <<"new Trajectory with " << currentTrajectory->getSteps()<<"\n"; + } + } + + // get next commanded trajectory type + if (currentTrajectory != NULL) + { + switch (currentTrajectory->getMovementType()) + { + case MovementCartBased: + { + currentCartPos = currentTrajectory->getNextCartPos(); + positionChanged = true; + currentMovementType = MovementCartBased; + } + break; + case MovementJointBased: + { + currentJointPos = currentTrajectory->getNextJointPos(); + positionChanged = true; + currentMovementType = MovementJointBased; + } + break; + default: + { + // invalid trajectory skip it + delete currentTrajectory; + currentTrajectory = NULL; + goto end; + } + break; + } + } + +end: + switch (currentMovementType) + { + case MovementJointBased: + { + // send the new joint values + float newJointPosToSend[jointNumber] = {0.0f}; + VecJoint newJointPosToSendRad = currentJointPos*(1./180*M_PI); + newJointPosToSendRad.getData(newJointPosToSend); + if (REAL_ROBOT) + friInst->doPositionControl(newJointPosToSend); + } + break; + case MovementCartBased: + { + float Stiff[6] = {1000.0, 1000.0, 1000.0, 10.0, 10.0, 10.0}; + float Damp[6] = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7}; + float newCartPosToSend[12] = {0.0f}; + currentCartPos.getData(newCartPosToSend); + if (REAL_ROBOT) + friInst->doCartesianImpedanceControl(newCartPosToSend,Stiff, Damp, NULL, NULL, true); + } + break; + } + + // start timer for the last sent msg + // TODO + + // check if current trajectory is over + if(currentTrajectory != NULL && currentTrajectory->getRemainingSteps() == 0) + { + std::cout << " Trajectory finished \n "; + + static int trajcetorycount = 0; + trajcetorycount +=1; + + currentTrajectory->saveToFile(std::string("trajectory/a.csv")); + // invalid trajectory skip it + delete currentTrajectory; + currentTrajectory = NULL; + } + } +} +int main() +{ + int err = 0; + //Setting pthread for FRI interface + pthread_t fri_t; + + //Start the thread for the robot movement + err = pthread_create(&fri_t,NULL,&threadRobotMovement,NULL); + if (err > 0 ) + { + std::cerr << "Failed: could not create thread\n" << std::endl; + return err; + } + + //Start client handling + SvrHandling *svr = new SvrHandling(); + if (svr == NULL) + { + std::cerr << "Failed: could not create server \n" << std::endl; + return -ENOMEM; + } + svr->run(); + + return 0; +} diff --git a/lwrserv/src/program.cpp b/lwrserv/src/program.cpp new file mode 100644 index 0000000..b93d1f2 --- /dev/null +++ b/lwrserv/src/program.cpp @@ -0,0 +1,177 @@ +#include "sgn.h" +#include +#include +#include +#include + +#include "SvrData.h" +#include "SvrHandling.h" +#include + +#include "Mat.h" +#include "Vec.h" +#include "Robot.h" + +float* abctomat(float a, float b, float c) +{ + Mat4f rx; + float ca = cos(c); + float sa = sin(c); + + rx(0,0) = 1.0f; + rx(1,1) = ca; + rx(1,2) = -sa; + rx(2,1) = sa; + rx(2,2) = ca; + rx(3,3) = 1.0f; + + Mat4f ry; + float cb = cos(b); + float sb = sin(b); + ry(0,0) = cb; + ry(0,2) = sb; + ry(1,1) = 1.0f; + ry(2,0) = -sb; + ry(2,2) = cb; + ry(3,3) = 1.0f; + + Mat4f rz; + float cc = cos(a); + float sc = sin(a); + rz(0,0) = cc; + rz(0,1) = -sc; + rz(1,0) = sc; + rz(1,1) = cc; + rz(2,2) = 1.0f; + rz(3,3) = 1.0f; + + Mat4f result; + Mat4f temp; + temp = rz * ry; + result = temp * rx; + +#ifdef __DEBUG__ + printMat(rx); + printMat(ry); + printMat(rz); + printMat(result); +#endif + + float *res = new float[12]; + //TODO simple converter for mat + for (int j=0;j<3;j++) + { + for (int k=0;k<4;k++) + { + res[j*4+k] = result(j,k); + } + } + return res; +} + +float* mattoabc(float M[12]) +{ + float norm; + float sa; + float ca; + float *abc = new float[3]; + + norm = sqrt((M[0]*M[0])+(M[4]*M[4])); + + if (norm>1e-5) + { + sa = M[4]/norm; + ca = M[0]/norm; + abc[0] = atan2(sa,ca); + } + else + { + sa = 0; + ca = 1; + abc[0] = 0; + } + abc[1] = atan2(-M[8],ca*M[0]+sa*M[4]); + abc[2] = atan2(sa*M[2]-ca*M[6],-sa*M[1]+ca*M[5]); + return abc; +} + +Mat4f vecToMat2(float vec[12]) +{ + Mat4f result; + for (int i=0; i<3; i++) + { + for (int j=0; j<4; j++) + { + result(i,j) = (float)vec[i*4+j]; + } + } + result(3,3)=1.0f; + return result; +} + +float* matToVec2(Mat4f mat) +{ + float* vec = new float[12]; + for (int j=0;j<3;j++) + { + for (int k=0;k<4;k++) + { + vec[j*4+k] = mat(j,k); + } + } + return vec; +} + +float* vectoquat(float vec[12]) +{ + float *quat = new float[4]; + float diag[3]; + int v; int w; int u; + diag[0] = vec[0]; + diag[1] = vec[5]; + diag[2] = vec[10]; + float u_ = *std::max_element(diag,diag+2); + + if (u_==diag[0]) + { + u = 1; + v = 2; + w = 3; + } else if (u_==diag[1]) + { + u = 2; + v = 3; + w = 1; + } else + { + u = 3; + v = 1; + w = 2; + } + float r = sqrt(1+vec[(u-1)*4+(u-1)] - vec[(v-1)*4+(v-1)] - vec[(w-1)*4+(w-1)]); + quat[0] = (vec[(w-1)*4+(v-1)] - vec[(v-1)*4+(w-1)]) / (2*r); + quat[u] = r/2; + quat[v] = (vec[(u-1)*4+(v-1)] + vec[(v-1)*4+(u-1)]) / (2*r); + quat[w] = (vec[(w-1)*4+(u-1)] + vec[(u-1)*4+(w-1)]) / (2*r); + + return quat; +} + +float* quattovec(float quat[4]) +{ + float *vec = new float[12]; + vec[0] = quat[0]*quat[0]+quat[1]*quat[1]-quat[2]*quat[2]-quat[3]*quat[3]; + vec[4] = 2*(quat[1]*quat[2]+quat[0]*quat[3]); + vec[8] = 2*(quat[1]*quat[3]-quat[0]*quat[2]); + + vec[1] = 2*(quat[1]*quat[2]-quat[0]*quat[3]); + vec[5] = quat[0]*quat[0]-quat[1]*quat[1]+quat[2]*quat[2]-quat[3]*quat[3]; + vec[9] = 2*(quat[2]*quat[3]+quat[0]*quat[1]); + + vec[2] = 2*(quat[1]*quat[3]+quat[0]*quat[2]); + vec[6] = 2*(quat[2]*quat[3]-quat[0]*quat[1]); + vec[10] = quat[0]*quat[0]-quat[1]*quat[1]-quat[2]*quat[2]+quat[3]*quat[3]; + + return vec; +} +