diff --git a/include/FSM/State_Cartesian.h b/include/FSM/State_Cartesian.h
index bc88ec8..f12ba13 100755
--- a/include/FSM/State_Cartesian.h
+++ b/include/FSM/State_Cartesian.h
@@ -12,14 +12,14 @@ public:
     void exit();
     int checkChange(int cmd);
 private:
-    double _oriSpeed = 0.4;// control by keyboard or joystick
-    double _posSpeed = 0.15;
-    double oriSpeedLimit = 0.3;// limits in SDK
-    double posSpeedLimit = 0.3;
+    double _oriSpeed = 0.4;// control by keyboard
+    double _posSpeed = 0.3;
+    double oriSpeedLimit = 0.5;// limits in SDK
+    double posSpeedLimit = 0.5;
     VecX _changeDirections;
 
     Vec6 _twist;
-    HomoMat _endHomoGoal, _endHomoGoalPast;
+    HomoMat _endHomoGoal, _endHomoPast, _endHomoDelta;
     Vec6 _endPostureGoal, _endPosturePast, _endPostureDelta;
 };
 
diff --git a/include/FSM/State_MoveC.h b/include/FSM/State_MoveC.h
index c27a5f7..a226e61 100755
--- a/include/FSM/State_MoveC.h
+++ b/include/FSM/State_MoveC.h
@@ -17,6 +17,7 @@ private:
     std::vector<Vec6>  _postures;
     EndCircleTraj *_circleTraj;
     bool _timeReached, _taskReached, _pastTaskReached, _finalReached;
+    uint _taskCnt;
 };
 
 #endif  // CARTESIAN_H
\ No newline at end of file
diff --git a/include/FSM/State_MoveL.h b/include/FSM/State_MoveL.h
index fc16132..7d1a5a4 100755
--- a/include/FSM/State_MoveL.h
+++ b/include/FSM/State_MoveL.h
@@ -17,5 +17,6 @@ private:
     std::vector<Vec6>  _postures;
     EndLineTraj *_lineTraj;
     bool _timeReached, _taskReached, _pastTaskReached, _finalReached;
+    uint _taskCnt;
 };
 #endif  // CARTESIAN_H
\ No newline at end of file
diff --git a/include/common/enumClass.h b/include/common/enumClass.h
index b6542d0..548e3eb 100755
--- a/include/common/enumClass.h
+++ b/include/common/enumClass.h
@@ -33,8 +33,7 @@ enum class JointMotorType{
 
 enum class Control{
     KEYBOARD,
-    SDK,
-    JOYSTICK
+    SDK
 };
 
 #endif  // ENUMCLASS_H
\ No newline at end of file
diff --git a/include/control/CtrlComponents.h b/include/control/CtrlComponents.h
index bc508ae..89f9762 100755
--- a/include/control/CtrlComponents.h
+++ b/include/control/CtrlComponents.h
@@ -32,18 +32,19 @@ public:
     //config
     double dt;
     bool *running;
-    Control ctrl;
+    Control ctrl = Control::SDK;
     bool hasGripper;
     bool isCollisionOpen;
     double collisionTLimit;
     bool isPlot;
     int trajChoose = 1;
-    size_t dogType = 1;//1:Aliengo 2:B1
+    size_t armType = 36;
 
     void geneObj();
     void writeData();
 private:
     void inputProcess(int argc, char** argv);
+    void configProcess();
 
     std::string ctrl_IP;
     uint ctrl_port;
diff --git a/include/control/cmdPanel.h b/include/control/cmdPanel.h
index 66b9e96..6ed3a5a 100644
--- a/include/control/cmdPanel.h
+++ b/include/control/cmdPanel.h
@@ -56,12 +56,6 @@ private:
 
 };
 
-
-/* 
-必须为长按, deltaValue为每秒改变量 
-valueAction允许共用按键，例如空格停止
-但是正反转、停止键不可重复
-*/
 class ValueAction : public KeyAction{
 public:    
     ValueAction(std::string cUp, std::string cDown, double deltaValue, double initValue = 0.0);
diff --git a/include/control/joystick.h b/include/control/joystick.h
deleted file mode 100644
index f85d9c7..0000000
--- a/include/control/joystick.h
+++ /dev/null
@@ -1,30 +0,0 @@
-#ifndef _UNITREE_ARM_JOYSTICK_H_
-#define _UNITREE_ARM_JOYSTICK_H_
-
-#include <vector>
-#include "control/cmdPanel.h"
-#include "message/joystick_common.h"
-#include "message/aliengo_common.h"
-#include "message/b1_common.h"
-#include <math.h>
-#include "message/udp.h"
-
-class UnitreeJoystick : public CmdPanel{
-public:
-    UnitreeJoystick( size_t dogType, std::vector<KeyAction*> events, 
-        EmptyAction emptyAction, size_t channelNum = 1,
-        double dt = 0.002);
-    ~UnitreeJoystick();
-private:
-    void _read();
-    void _extractCmd();
-    size_t _dogType;
-    xRockerBtnDataStruct _keyData;
-    UDPPort *_udp;
-    UNITREE_LEGGED_SDK_ALIENGO::HighCmd _udpCmdAliengo;
-    UNITREE_LEGGED_SDK_ALIENGO::HighState _udpStateAliengo;
-    UNITREE_LEGGED_SDK_B1::HighCmd _udpCmdB1;
-    UNITREE_LEGGED_SDK_B1::HighState _udpStateB1;
-};
-
-#endif  // _UNITREE_ARM_JOYSTICK_H_
diff --git a/include/interface/IOInterface.h b/include/interface/IOInterface.h
index 50571dc..9af2a71 100755
--- a/include/interface/IOInterface.h
+++ b/include/interface/IOInterface.h
@@ -5,7 +5,6 @@
 #include "message/LowlevelCmd.h"
 #include "message/LowlevelState.h"
 #include "control/keyboard.h"
-#include "control/joystick.h"
 #include "common/math/robotics.h"
 
 class IOInterface{
diff --git a/include/message/aliengo_common.h b/include/message/aliengo_common.h
deleted file mode 100644
index b82e925..0000000
--- a/include/message/aliengo_common.h
+++ /dev/null
@@ -1,168 +0,0 @@
-#ifndef _UNITREE_ARM_ALIENGO_COMMON_H_
-#define _UNITREE_ARM_ALIENGO_COMMON_H_
-
-#include <stdint.h>
-
-namespace UNITREE_LEGGED_SDK_ALIENGO
-{
-
-	constexpr int HIGHLEVEL = 0x00;
-	constexpr int LOWLEVEL  = 0xff;
-	constexpr double PosStopF = (2.146E+9f);
-	constexpr double VelStopF = (16000.0f);
-
-#pragma pack(1)
-
-	// 12 bytes
-	typedef struct
-	{
-		float x;
-		float y;
-		float z;
-	} Cartesian;
-
-	// 53 bytes
-	typedef struct
-	{
-		float quaternion[4];               // quaternion, normalized, (w,x,y,z)
-		float gyroscope[3];                // angular velocity （unit: rad/s)
-		float accelerometer[3];            // m/(s2)
-		float rpy[3];                      // euler angle（unit: rad)
-		int8_t temperature;
-	} IMU;                                 // when under accelerated motion, the attitude of the robot calculated by IMU will drift.
-
-	// 3 bytes
-	typedef struct
-	{
-		uint8_t r;
-		uint8_t g;
-		uint8_t b;
-	} LED;                                 // foot led brightness: 0~255
-
-	// 38 bytes
-	typedef struct
-	{
-		uint8_t mode;                      // motor working mode 
-		float q;                           // current angle (unit: radian)
-		float dq;                          // current velocity (unit: radian/second)
-		float ddq;                       // current acc (unit: radian/second*second)
-		float tauEst;                    // current estimated output torque (unit: N.m)
-		float q_raw;                       // current angle (unit: radian)
-		float dq_raw;                      // current velocity (unit: radian/second)
-		float ddq_raw;
-		int8_t temperature;                // current temperature (temperature conduction is slow that leads to lag)
-        uint32_t reserve[2];   // in reserve[1] returns the brake state.
-	} MotorState;                          // motor feedback
-
-	// 33 bytes
-	typedef struct
-	{
-		uint8_t mode;                      // desired working mode
-		float q;                           // desired angle (unit: radian) 
-		float dq;                          // desired velocity (unit: radian/second)
-		float tau;                       // desired output torque (unit: N.m)
-		float Kp;                        // desired position stiffness (unit: N.m/rad )
-		float Kd;                        // desired velocity stiffness (unit: N.m/(rad/s) )
-        uint32_t reserve[3]; // in reserve[0] sends the brake cmd.
-	} MotorCmd;                            // motor control
-
-	// 891 bytes
-	typedef struct
-	{
-		uint8_t levelFlag;                 // flag to distinguish high level or low level
-		uint16_t commVersion;
-		uint16_t robotID;
-		uint32_t SN; 
-		uint8_t bandWidth;
-		IMU imu;
-		MotorState motorState[20];
-		int16_t footForce[4];              // force sensors
-		int16_t footForceEst[4];           // force sensors
-		uint32_t tick;                     // reference real-time from motion controller (unit: us)
-		uint8_t wirelessRemote[40];        // wireless commands
-		uint32_t reserve;
-		uint32_t crc;
-	} LowState;                            // low level feedback
-
-	// 730 bytes
-	typedef struct 
-	{
-		uint8_t levelFlag;
-		uint16_t commVersion;
-		uint16_t robotID;
-		uint32_t SN;
-		uint8_t bandWidth;
-		MotorCmd motorCmd[20];
-		LED led[4];
-		uint8_t wirelessRemote[40];
-		uint32_t reserve;
-		uint32_t crc;
-	} LowCmd;                              // low level control
-
-	// 244byte
-	typedef struct
-	{
-        uint8_t levelFlag;
-        uint16_t commVersion;
-        uint16_t robotID;
-        uint32_t SN;
-        uint8_t bandWidth;
-        uint8_t mode;
-        IMU imu;
-        float position[3];                 // (unit: m), from robot own odometry in inertial frame, usually drift
-        float velocity[3];                 // (unit: m/s), forwardSpeed, sideSpeed, updownSpeed in body frame
-        float yawSpeed;                    // (unit: rad/s), rotateSpeed in body frame.
-        Cartesian footPosition2Body[4];    // foot position relative to body
-        Cartesian footSpeed2Body[4];       // foot speed relative to body
-        int16_t footForce[4];
-        uint8_t wirelessRemote[40];
-        uint32_t reserve;
-        uint32_t crc;
-	} HighState;                           // high level feedback
-
-	//113byte
-	typedef struct
-	{
-        uint8_t levelFlag;
-        uint16_t commVersion;
-        uint16_t robotID;
-        uint32_t SN;
-        uint8_t bandWidth;
-        uint8_t mode;                      // 0.idle, default stand | 1.force stand (controlled by dBodyHeight + rpy)
-                                           // 2.target velocity walking (controlled by velocity + yawSpeed)
-                                           // 3.target position walking (controlled by position + rpy[2])
-                                           // 4. path mode walking (reserve for future release)
-                                           // 5. position stand down. |6. position stand up |7. damping mode | 8. recovery mode
-        uint8_t gaitType;                  // 0.trot | 1. trot running  | 2.climb stair
-        uint8_t speedLevel;                // 0. default low speed. 1. medium speed 2. high speed. during walking
-        float dFootRaiseHeight;            // (unit: m), swing foot height adjustment from default swing height.
-        float dBodyHeight;                 // (unit: m), body height adjustment from default body height.
-        float position[2];                 // (unit: m), desired x and y position in inertial frame.
-        float rpy[3];                      // (unit: rad), desired yaw-pitch-roll euler angle, expressed in roll(rpy[0]) pitch(rpy[1]) yaw(rpy[2])
-        float velocity[2];                 // (unit: m/s), forwardSpeed, sideSpeed in body frame.
-        float yawSpeed;                    // (unit: rad/s), rotateSpeed in body frame.
-        LED led[4];
-        uint8_t wirelessRemote[40];
-        uint32_t reserve;
-        uint32_t crc;
-    } HighCmd;                             // high level control         uint8_t mode
-
-#pragma pack()
-
-	typedef struct     
-	{
-		unsigned long long TotalCount;     // total loop count
-		unsigned long long SendCount;      // total send count
-		unsigned long long RecvCount;      // total receive count
-		unsigned long long SendError;      // total send error 
-		unsigned long long FlagError;      // total flag error 
-		unsigned long long RecvCRCError;   // total reveive CRC error	
-		unsigned long long RecvLoseError;  // total lose package count	
-	} UDPState;                            // UDP communication state
-
-	constexpr int HIGH_CMD_LENGTH   = (sizeof(HighCmd));
-	constexpr int HIGH_STATE_LENGTH = (sizeof(HighState));
-	
-}
-
-#endif
\ No newline at end of file
diff --git a/include/message/arm_common.h b/include/message/arm_common.h
index e8e15b1..5b101fb 100644
--- a/include/message/arm_common.h
+++ b/include/message/arm_common.h
@@ -26,19 +26,6 @@ enum class ArmFSMState{
     LOWCMD
 };
 
-// 4 Byte
-enum class ArmFSMValue{
-    INVALID,
-    Q,A,
-    W,S,  
-    E,D,
-    R,F,
-    T,G, 
-    Y,H,   
-    DOWN,
-    UP
-};
-
 enum class TrajType{
     MoveJ,
     MoveL,
diff --git a/include/message/b1_common.h b/include/message/b1_common.h
deleted file mode 100644
index 24debcd..0000000
--- a/include/message/b1_common.h
+++ /dev/null
@@ -1,211 +0,0 @@
-#ifndef _UNITREE_ARM_B1_COMMON_H_
-#define _UNITREE_ARM_B1_COMMON_H_
-
-#include <stdint.h>
-#include <array>
-
-namespace UNITREE_LEGGED_SDK_B1
-{
-
-	constexpr int HIGHLEVEL    = 0xee;
-	constexpr int LOWLEVEL     = 0xff;
-	constexpr int TRIGERLEVEL  = 0xf0;
-	constexpr double PosStopF  = (2.146E+9f);
-	constexpr double VelStopF  = (16000.0f);
-	extern const int LOW_CMD_LENGTH;       // shorter than sizeof(LowCmd),   bytes compressed LowCmd length  
-	extern const int LOW_STATE_LENGTH;     // shorter than sizeof(LowState), bytes compressed LowState length
-
-#pragma pack(1)
-
-	typedef struct
-	{
-		uint8_t off;                       // off 0xA5
-		std::array<uint8_t, 3> reserve;
-	} BmsCmd;
-
-	typedef struct
-	{
-		uint8_t version_h;
-		uint8_t version_l;
-		uint8_t bms_status;
-		uint8_t SOC;                       // SOC 0-100%
-		int32_t current;                   // mA
-		uint16_t cycle;
-		std::array<int8_t, 8> BQ_NTC;                  // x1 degrees centigrade
-		std::array<int8_t, 8> MCU_NTC;                 // x1 degrees centigrade
-		std::array<uint16_t, 30>  cell_vol;            // cell voltage mV
-	} BmsState;
-
-	typedef struct
-	{
-		float x;
-		float y;
-		float z;
-	} Cartesian;
-
-	typedef struct
-	{
-		std::array<float, 4> quaternion;               // quaternion, normalized, (w,x,y,z)
-		std::array<float, 3> gyroscope;                // angular velocity （unit: rad/s)    (raw data)
-		std::array<float, 3> accelerometer;            // m/(s2)                             (raw data)
-		std::array<float, 3> rpy;                      // euler angle（unit: rad)
-		int8_t temperature;
-	} IMU;                                 // when under accelerated motion, the attitude of the robot calculated by IMU will drift.
-
-	typedef struct
-	{
-		uint8_t r;
-		uint8_t g;
-		uint8_t b;
-	} LED;                                 // foot led brightness: 0~255
-
-	typedef struct
-	{
-		uint8_t mode;                      // motor working mode 
-		float q;                           // current angle (unit: radian)
-		float dq;                          // current velocity (unit: radian/second)
-		float ddq;                         // current acc (unit: radian/second*second)
-		float tauEst;                      // current estimated output torque (unit: N.m)
-		float q_raw;                       // current angle (unit: radian)
-		float dq_raw;                      // current velocity (unit: radian/second)
-		float ddq_raw;
-		int8_t temperature;                // current temperature (temperature conduction is slow that leads to lag)
-		std::array<uint32_t, 2> reserve;
-	} MotorState;                          // motor feedback
-
-	typedef struct
-	{
-		uint8_t mode;                      // desired working mode
-		float q;                           // desired angle (unit: radian) 
-		float dq;                          // desired velocity (unit: radian/second)
-		float tau;                         // desired output torque (unit: N.m)
-		float Kp;                          // desired position stiffness (unit: N.m/rad )
-		float Kd;                          // desired velocity stiffness (unit: N.m/(rad/s) )
-		std::array<uint32_t, 3> reserve;
-	} MotorCmd;                            // motor control
-
-	typedef struct
-	{
-		std::array<uint8_t, 2> head;
-		uint8_t levelFlag;
-		uint8_t frameReserve;
-		
-		std::array<uint32_t, 2> SN;
-		std::array<uint32_t, 2> version;
-		uint16_t bandWidth;
-		IMU imu;
-		std::array<MotorState, 20> motorState;
-		BmsState bms;
-		std::array<int16_t, 4> footForce;           // force sensors
-		std::array<int16_t, 4> footForceEst;        // force sensors
-		uint32_t tick;                              // reference real-time from motion controller (unit: us)
-		std::array<uint8_t, 40> wirelessRemote;     // wireless commands
-		uint32_t reserve;
-		
-		uint32_t crc;
-	} LowState;                            // low level feedback
-
-	typedef struct
-	{
-		std::array<uint8_t, 2> head;
-		uint8_t levelFlag;
-		uint8_t frameReserve;
-		
-		std::array<uint32_t, 2> SN;
-		std::array<uint32_t, 2> version;
-		uint16_t bandWidth;
-		std::array<MotorCmd, 20> motorCmd;
-		BmsCmd bms;
-		std::array<uint8_t, 40> wirelessRemote;
-		uint32_t reserve;
-		
-		uint32_t crc;
-	} LowCmd;                              // low level control
-
-	typedef struct
-	{
-		std::array<uint8_t, 2> head;
-		uint8_t levelFlag;
-		uint8_t frameReserve;
-		
-		std::array<uint32_t, 2> SN;
-		std::array<uint32_t, 2> version;
-		uint16_t bandWidth;
-		IMU imu;
-		std::array<MotorState, 20> motorState;
-		BmsState bms;
-		std::array<int16_t, 4> footForce;
-		std::array<int16_t, 4> footForceEst;
-		uint8_t mode;
-		float progress;
-		uint8_t gaitType;                            // 0.idle  1.trot  2.trot running  3.climb stair  4.trot obstacle
-		float footRaiseHeight;                       // (unit: m, default: 0.08m), foot up height while walking
-		std::array<float, 3> position;               // (unit: m), from own odometry in inertial frame, usually drift
-		float bodyHeight;                            // (unit: m, default: 0.28m),
-		std::array<float, 3> velocity;               // (unit: m/s), forwardSpeed, sideSpeed, rotateSpeed in body frame
-		float yawSpeed;                              // (unit: rad/s), rotateSpeed in body frame        
-		std::array<float, 4> rangeObstacle;
-		std::array<Cartesian, 4> footPosition2Body;  // foot position relative to body
-		std::array<Cartesian, 4> footSpeed2Body;     // foot speed relative to body
-		std::array<uint8_t, 40> wirelessRemote;
-		uint32_t reserve;
-		
-		uint32_t crc;
-	} HighState;                           // high level feedback
-
-	typedef struct
-	{
-		std::array<uint8_t, 2> head;
-		uint8_t levelFlag;
-		uint8_t frameReserve;
-		
-		std::array<uint32_t, 2> SN;
-		std::array<uint32_t, 2> version;
-		uint16_t bandWidth;
-		uint8_t mode;                       // 0. idle, default stand  1. force stand (controlled by dBodyHeight + ypr)
-											// 2. target velocity walking (controlled by velocity + yawSpeed)
-											// 3. target position walking (controlled by position + ypr[0])
-											// 4. path mode walking (reserve for future release)
-											// 5. position stand down. 
-											// 6. position stand up 
-											// 7. damping mode 
-											// 8. recovery stand
-											// 9. backflip
-											// 10. jumpYaw
-											// 11. straightHand
-											// 12. dance1
-											// 13. dance2
-
-		uint8_t gaitType;                   // 0.idle  1.trot  2.trot running  3.climb stair  4.trot obstacle
-		uint8_t speedLevel;                 // 0. default low speed. 1. medium speed 2. high speed. during walking, only respond MODE 3
-		float footRaiseHeight;              // (unit: m, default: 0.08m), foot up height while walking, delta value
-		float bodyHeight;                   // (unit: m, default: 0.28m), delta value
-		std::array<float, 2> position;       // (unit: m), desired position in inertial frame
-		std::array<float, 3> euler;         // (unit: rad), roll pitch yaw in stand mode
-		std::array<float, 2> velocity;      // (unit: m/s), forwardSpeed, sideSpeed in body frame
-		float yawSpeed;                     // (unit: rad/s), rotateSpeed in body frame
-		BmsCmd bms;
-		std::array<LED, 4> led;
-		std::array<uint8_t, 40> wirelessRemote;
-		uint32_t reserve;
-		
-		uint32_t crc;
-	} HighCmd;                              // high level control
-
-#pragma pack()
-
-	typedef struct
-	{
-		unsigned long long TotalCount;     // total loop count
-		unsigned long long SendCount;      // total send count
-		unsigned long long RecvCount;      // total receive count
-		unsigned long long SendError;      // total send error 
-		unsigned long long FlagError;      // total flag error 
-		unsigned long long RecvCRCError;   // total reveive CRC error	
-		unsigned long long RecvLoseError;  // total lose package count	
-	} UDPState;                            // UDP communication state
-	constexpr int HIGH_CMD_LENGTH   = (sizeof(HighCmd));
-	constexpr int HIGH_STATE_LENGTH = (sizeof(HighState));
-}
-
-#endif
\ No newline at end of file
diff --git a/include/message/joystick_common.h b/include/message/joystick_common.h
deleted file mode 100644
index b9c1e06..0000000
--- a/include/message/joystick_common.h
+++ /dev/null
@@ -1,43 +0,0 @@
-/*****************************************************************
-Copyright (c) 2022, Unitree Robotics.Co.Ltd. All rights reserved.
-*****************************************************************/
-#ifndef _UNITREE_ARM_JOYSTICK_COMMON_H_
-#define _UNITREE_ARM_JOYSTICK_COMMON_H_
-#include <stdint.h>
-
-// 16b
-typedef union {
-    struct {
-        uint8_t R1          :1;
-        uint8_t L1          :1;
-        uint8_t start       :1;
-        uint8_t select      :1;
-        uint8_t R2          :1;
-        uint8_t L2          :1;
-        uint8_t F1          :1;
-        uint8_t F2          :1;
-        uint8_t A           :1;
-        uint8_t B           :1;
-        uint8_t X           :1;
-        uint8_t Y           :1;
-        uint8_t up          :1;
-        uint8_t right       :1;
-        uint8_t down        :1;
-        uint8_t left        :1;
-    } components;
-    uint16_t value;
-} xKeySwitchUnion;
-
-// 40 Byte (now used 24B)
-typedef struct {
-    uint8_t head[2];
-    xKeySwitchUnion btn;
-    float lx;
-    float rx;
-    float ry;
-    float L2;
-    float ly;
-    uint8_t idle[16];
-} xRockerBtnDataStruct;
-
-#endif
\ No newline at end of file
diff --git a/include/model/ArmModel.h b/include/model/ArmModel.h
index 5650cd6..acc5353 100644
--- a/include/model/ArmModel.h
+++ b/include/model/ArmModel.h
@@ -14,7 +14,8 @@ public:
 
     HomoMat forwardKinematics(Vec6 q, int index = 6);
     virtual bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWorkSpace = false);
-    Mat6 CalcJacobian(Vec6 q);
+    Mat6 CalcJacobianSpace(Vec6 q);
+    Mat6 CalcJacobianBody(Vec6 q);
     Vec6 inverseDynamics(Vec6 q, Vec6 qd, Vec6 qdd, Vec6 Ftip);
     virtual void solveQP(Vec6 twist, Vec6 qPast, Vec6& qd_result, double dt) = 0;
 
@@ -58,7 +59,7 @@ protected:
 
 class Z1Model : public ArmModel{
 public:
-    Z1Model(Vec3 endPosLocal = Vec3::Zero(), double endEffectorMass = 0.0,
+    Z1Model(size_t armType = 36, Vec3 endPosLocal = Vec3::Zero(), double endEffectorMass = 0.0,
             Vec3 endEffectorCom = Vec3::Zero(), Mat3 endEffectorInertia = Mat3::Zero());
     ~Z1Model(){};
 
diff --git a/include/trajectory/Trajectory.h b/include/trajectory/Trajectory.h
index eaf40dc..2630bbd 100755
--- a/include/trajectory/Trajectory.h
+++ b/include/trajectory/Trajectory.h
@@ -41,7 +41,6 @@ protected:
     HomoMat _startHomo, _endHomo;
     Vec6 _startPosture, _endPosture, _deltaPosture;
     double _startGripperQ, _endGripperQ;
-
 };
 
 #endif  // TRAJECTORY_H
\ No newline at end of file
diff --git a/lib/libZ1_ROS_Linux64.so b/lib/libZ1_ROS_Linux64.so
index 83f8619..21adce2 100644
Binary files a/lib/libZ1_ROS_Linux64.so and b/lib/libZ1_ROS_Linux64.so differ
diff --git a/lib/libZ1_UDP_Linux64.so b/lib/libZ1_UDP_Linux64.so
index 5eb4fd8..6a78c90 100644
Binary files a/lib/libZ1_UDP_Linux64.so and b/lib/libZ1_UDP_Linux64.so differ
diff --git a/main.cpp b/main.cpp
index 22fc687..3e3639a 100755
--- a/main.cpp
+++ b/main.cpp
@@ -39,7 +39,7 @@ int main(int argc, char **argv){
     /* set real-time process */
     setProcessScheduler();
     /* set the print format */
-    std::cout << std::fixed << std::setprecision(3);
+    std::cout << std::fixed << std::setprecision(5);
 
 
     EmptyAction emptyAction((int)ArmFSMStateName::INVALID);
@@ -80,22 +80,6 @@ int main(int argc, char **argv){
         events.push_back(new ValueAction("down", "up", 1.));
 
         ctrlComp->cmdPanel = new Keyboard(events, emptyAction);
-    }else if(ctrlComp->ctrl == Control::JOYSTICK){
-        events.push_back(new StateAction("r2x",     (int)ArmFSMStateName::TRAJECTORY));
-        events.push_back(new StateAction("l12",     (int)ArmFSMStateName::PASSIVE));
-        events.push_back(new StateAction("r2",      (int)ArmFSMStateName::JOINTCTRL));
-        events.push_back(new StateAction("r1",      (int)ArmFSMStateName::CARTESIAN));
-        events.push_back(new StateAction("select",  (int)ArmFSMStateName::BACKTOSTART));
-
-        events.push_back(new ValueAction("left_up",     "left_down",    0.5));//Tran_Y
-        events.push_back(new ValueAction("left_right",   "left_left",   0.5));//Tran_X, inverse
-        events.push_back(new ValueAction("up",          "down",         -0.5));//Tran_Z, inverse
-        events.push_back(new ValueAction("right_up",    "right_down",   0.5));//Rot_Y
-        events.push_back(new ValueAction("right_left",  "right_right",  0.5));//Rot_x
-        events.push_back(new ValueAction("Y",           "A",            0.5));//Rot_Z
-        events.push_back(new ValueAction("right",       "left",         1.0));//girpper, close-open
-
-        ctrlComp->cmdPanel = new UnitreeJoystick(ctrlComp->dogType, events, emptyAction);
     }
     std::vector<FSMState*> states;
     states.push_back(new State_Passive(ctrlComp));
@@ -112,7 +96,7 @@ int main(int argc, char **argv){
     states.push_back(new State_ToState(ctrlComp));
     states.push_back(new State_Trajectory(ctrlComp));
     states.push_back(new State_Calibration(ctrlComp));
-    states.push_back(new State_SetTraj(ctrlComp));
+    // states.push_back(new State_SetTraj(ctrlComp));
 
     FiniteStateMachine *fsm;
     fsm = new FiniteStateMachine(states, ctrlComp);