EmbodiedAI
/
z1_sdk
mirror of https://github.com/unitreerobotics/z1_sdk.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Revert "add wait flag" 

This reverts commit be77f00cfb.
This commit is contained in:
Agnel Wang 2022-12-05 12:17:23 +08:00
parent 83a4594b25
commit 2a627eac9e
16 changed files with 28 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
examples/bigDemo.cpp
View File

@ -1,4 +1,4 @@
#include "unitree_arm_sdk/control/unitreeArm.h" #include "control/unitreeArm.h"
using namespace UNITREE_ARM; using namespace UNITREE_ARM;
@ -63,7 +63,7 @@ void Z1ARM::armCtrlTrackInCartesian(){
endPosture(5) -= _ctrlComp->dt * 0.2;//z axis, m/s endPosture(5) -= _ctrlComp->dt * 0.2;//z axis, m/s
// no inverse kinematics solution, the joint has reached limit // no inverse kinematics solution, the joint has reached limit
// std::cout << "postureCmd: " << endPosture.transpose() << " qState: " << lowstate->endPosture.transpose() << std::endl; std::cout << "postureCmd: " << endPosture.transpose() << " qState: " << lowstate->endPosture.transpose() << std::endl;
double error = fabs(endPosture(5) - lowstate->endPosture(5)); double error = fabs(endPosture(5) - lowstate->endPosture(5));
if( error > 0.1){ if( error > 0.1){
break; break;
@ -91,7 +91,7 @@ int main() {
arm.backToStart(); arm.backToStart();
// size_t demo = 1; // size_t demo = 3;
for(size_t demo = 1; demo < 4; demo++) for(size_t demo = 1; demo < 4; demo++)
switch (demo) switch (demo)
{ {

8
examples/example_CtrlByFSM.cpp
View File

@ -1,4 +1,4 @@
#include "unitree_arm_sdk/control/unitreeArm.h" #include "control/unitreeArm.h"
using namespace UNITREE_ARM; using namespace UNITREE_ARM;
@ -16,23 +16,17 @@ int main() {
std::cout << "[TO STATE]" << std::endl; std::cout << "[TO STATE]" << std::endl;
arm.labelRun("forward"); arm.labelRun("forward");
std::cout << "[MOVEJ]" << std::endl; std::cout << "[MOVEJ]" << std::endl;
posture[0]<<0.5,0.1,0.1,0.5,-0.2,0.5; posture[0]<<0.5,0.1,0.1,0.5,-0.2,0.5;
arm.MoveJ(posture[0], -M_PI/3.0, 1.0); arm.MoveJ(posture[0], -M_PI/3.0, 1.0);
std::cout << "[MOVEL]" << std::endl; std::cout << "[MOVEL]" << std::endl;
posture[0] << 0,0,0,0.45,-0.2,0.2; posture[0] << 0,0,0,0.45,-0.2,0.2;
arm.setWait(false);
arm.MoveL(posture[0], 0., 0.3); arm.MoveL(posture[0], 0., 0.3);
while(arm._ctrlComp->recvState.state != ArmFSMState::JOINTCTRL){
usleep(2000);
}
std::cout << "[MOVEC]" << std::endl; std::cout << "[MOVEC]" << std::endl;
posture[0] << 0,0,0,0.45,0,0.4; posture[0] << 0,0,0,0.45,0,0.4;
posture[1] << 0,0,0,0.45,0.2,0.2; posture[1] << 0,0,0,0.45,0.2,0.2;
arm.setWait(true);
arm.MoveC(posture[0], posture[1], -M_PI/3.0, 0.3); arm.MoveC(posture[0], posture[1], -M_PI/3.0, 0.3);
//stop //stop

10
examples/example_JointCtrl.cpp
View File

@ -1,4 +1,4 @@
#include "unitree_arm_sdk/control/unitreeArm.h" #include "control/unitreeArm.h"
using namespace UNITREE_ARM; using namespace UNITREE_ARM;
@ -6,8 +6,6 @@ class JointTraj{
public: public:
JointTraj(){}; JointTraj(){};
~JointTraj(){}; ~JointTraj(){};
// set _pathTime and a3, a4, a5
void setJointTraj(Vec6 startQ, Vec6 endQ, double speed){ void setJointTraj(Vec6 startQ, Vec6 endQ, double speed){
_startQ = startQ; _startQ = startQ;
_endQ = endQ; _endQ = endQ;
@ -18,7 +16,6 @@ public:
} }
_generateA345(); _generateA345();
}; };
void setGripper(double startQ, double endQ, double speed){ void setGripper(double startQ, double endQ, double speed){
_gripperStartQ = startQ; _gripperStartQ = startQ;
_gripperEndQ = endQ; _gripperEndQ = endQ;
@ -63,10 +60,6 @@ private:
_reached = (_tCost>_pathTime) ? true : false; _reached = (_tCost>_pathTime) ? true : false;
_tCost = (_tCost>_pathTime) ? _pathTime : _tCost; _tCost = (_tCost>_pathTime) ? _pathTime : _tCost;
} }
// caculate the coefficients of the quintuple polynomial
// s(t) = a_0 + a_1*t + a_2*t^2 + a_3*t^3 + a_4*t^4 +a_5*t^5
// constraints: s(0) = 0; sdot(0) = 0; sdotdot(0) = 0; s(T) = 1; sdot(T) = 0; sdotdot(T) = 0
void _generateA345(){ void _generateA345(){
if(NearZero(_pathTime)){ if(NearZero(_pathTime)){
_a3 = 0; _a3 = 0;
@ -114,7 +107,6 @@ int main(){
forward << 0.0, 1.5, -1.0, -0.54, 0.0, 0.0; forward << 0.0, 1.5, -1.0, -0.54, 0.0, 0.0;
double speed = 0.5; double speed = 0.5;
arm.startTrack(ArmFSMState::JOINTCTRL); arm.startTrack(ArmFSMState::JOINTCTRL);
// move from current posture to [forward]
arm.jointTraj.setJointTraj(arm._ctrlComp->lowstate->getQ(), forward, speed); arm.jointTraj.setJointTraj(arm._ctrlComp->lowstate->getQ(), forward, speed);
arm.jointTraj.setGripper(arm._ctrlComp->lowstate->getGripperQ(), -1, speed); arm.jointTraj.setGripper(arm._ctrlComp->lowstate->getGripperQ(), -1, speed);

2
examples/example_lowCmd_send.cpp
View File

@ -1,4 +1,4 @@
#include "unitree_arm_sdk/control/unitreeArm.h" #include "control/unitreeArm.h"
using namespace UNITREE_ARM; using namespace UNITREE_ARM;

12
include/unitree_arm_sdk/control/ctrlComponents.h
View File

@ -1,12 +1,12 @@
#ifndef CTRLCOMPONENTS_H #ifndef CTRLCOMPONENTS_H
#define CTRLCOMPONENTS_H #define CTRLCOMPONENTS_H
#include "unitree_arm_sdk/message/arm_common.h" #include "message/arm_common.h"
#include "unitree_arm_sdk/message/LowlevelCmd.h" #include "message/LowlevelCmd.h"
#include "unitree_arm_sdk/message/LowlevelState.h" #include "message/LowlevelState.h"
#include "unitree_arm_sdk/message/udp.h" #include "message/udp.h"
#include "unitree_arm_sdk/utilities/loop.h" #include "utilities/loop.h"
#include "unitree_arm_sdk/model/ArmModel.h" #include "model/ArmModel.h"
namespace UNITREE_ARM { namespace UNITREE_ARM {
struct CtrlComponents{ struct CtrlComponents{

29
include/unitree_arm_sdk/control/unitreeArm.h
View File

@ -1,7 +1,7 @@
#ifndef __UNITREEARM_H #ifndef __UNITREEARM_H
#define __UNITREEARM_H #define __UNITREEARM_H
#include "unitree_arm_sdk/control/ctrlComponents.h" #include "control/ctrlComponents.h"
namespace UNITREE_ARM { namespace UNITREE_ARM {
@ -77,7 +77,7 @@ bool MoveJ(Vec6 posture, double maxSpeed);
* Input: posture: target position, (roll pitch yaw x y z), unit: meter * Input: posture: target position, (roll pitch yaw x y z), unit: meter
* gripperPos: target angular * gripperPos: target angular
* uint: radian * uint: radian
* range:[-pi/2, 0] * range:[0, pi/3]
* maxSpeed: the maximum joint speed when robot is moving * maxSpeed: the maximum joint speed when robot is moving
* unit: radian/s * unit: radian/s
* range:[0, pi] * range:[0, pi]
@ -95,7 +95,7 @@ bool MoveL(Vec6 posture, double maxSpeed);
* Function: Move the robot in a linear path, and control the gripper at the same time * Function: Move the robot in a linear path, and control the gripper at the same time
* Input: posture: target position, (roll pitch yaw x y z), unit: meter * Input: posture: target position, (roll pitch yaw x y z), unit: meter
* gripperPos: target angular, uint: radian * gripperPos: target angular, uint: radian
* range:[-pi/2, 0] * range:[0, pi/3]
* maxSpeed: the maximum joint speed when robot is moving, unit: m/s * maxSpeed: the maximum joint speed when robot is moving, unit: m/s
* Output: whether posture has inverse kinematics * Output: whether posture has inverse kinematics
*/ */
@ -113,7 +113,7 @@ bool MoveC(Vec6 middlePosutre, Vec6 endPosture, double maxSpeed);
* Input: middle posture: determine the shape of the circular path * Input: middle posture: determine the shape of the circular path
* endPosture: target position, (roll pitch yaw x y z), unit: meter * endPosture: target position, (roll pitch yaw x y z), unit: meter
* gripperPos: target angular, uint: radian * gripperPos: target angular, uint: radian
* range:[-pi/2, 0] * range:[0, pi/3]
* maxSpeed: the maximum joint speed when robot is moving, unit: m/s * maxSpeed: the maximum joint speed when robot is moving, unit: m/s
* Output: whether posture has inverse kinematics * Output: whether posture has inverse kinematics
*/ */
@ -153,24 +153,6 @@ void startTrack(ArmFSMState fsm);
*/ */
void sendRecv(); void sendRecv();
/*
* Function: whether to wait for the command to finish
* Input: true or false
* Output: None
* Description: For example, MoveJ will send a trajectory command to z1_controller and then
* run usleep() to wait for the trajectory execution to complete.
* If set [wait] to false, MoveJ will send command only and user should judge
* for youself whether the command is complete.
* Method 1: if(_ctrlComp->recvState.state != fsm)
* After trajectory complete, the FSM will switch to ArmFSMState::JOINTCTRL
* automatically
* Method 2: if((lowState->endPosture - endPostureGoal).norm() < error)
* Check whether current posture reaches the target
* Related functions:
* MoveJ(), MoveL(), MoveC(), backToStart(), labelRun(), teachRepeat()
*/
void setWait(bool Y_N);
//command parameters //command parameters
Vec6 q, qd, tau; Vec6 q, qd, tau;
Vec6 endPosture; Vec6 endPosture;
@ -180,9 +162,6 @@ LoopFunc *sendRecvThread;
LowlevelCmd *lowcmd;//same as _ctrlComp->lowcmd LowlevelCmd *lowcmd;//same as _ctrlComp->lowcmd
LowlevelState *lowstate;//same as _ctrlComp->lowstate LowlevelState *lowstate;//same as _ctrlComp->lowstate
CtrlComponents *_ctrlComp; CtrlComponents *_ctrlComp;
private:
bool _isWait = true;
}; };
} }

2
include/unitree_arm_sdk/math/mathTools.h
View File

@ -3,7 +3,7 @@
#include <iostream> #include <iostream>
#include <math.h> #include <math.h>
#include "unitree_arm_sdk/math/mathTypes.h" #include "math/mathTypes.h"
namespace UNITREE_ARM { namespace UNITREE_ARM {

2
include/unitree_arm_sdk/message/LowlevelCmd.h
View File

@ -1,7 +1,7 @@
#ifndef LOWLEVELCMD_H #ifndef LOWLEVELCMD_H
#define LOWLEVELCMD_H #define LOWLEVELCMD_H
#include "unitree_arm_sdk/math/mathTypes.h" #include "math/mathTypes.h"
#include <vector> #include <vector>
#include <iostream> #include <iostream>

4
include/unitree_arm_sdk/message/LowlevelState.h
View File

@ -3,8 +3,8 @@
#include <iostream> #include <iostream>
#include <vector> #include <vector>
#include "unitree_arm_sdk/math/mathTypes.h" #include "math/mathTypes.h"
#include "unitree_arm_sdk/message/arm_common.h" #include "message/arm_common.h"
namespace UNITREE_ARM { namespace UNITREE_ARM {

2
include/unitree_arm_sdk/message/arm_common.h
View File

@ -83,7 +83,7 @@ struct TrajCmd{
double gripperPos; double gripperPos;
double maxSpeed; double maxSpeed;
double stopTime; double stopTime;
int reserved; int trajOrder;
}; };
union ValueUnion{ union ValueUnion{

2
include/unitree_arm_sdk/message/udp.h
View File

@ -6,7 +6,7 @@
#include <string> #include <string>
#include <string.h> #include <string.h>
#include <vector> #include <vector>
#include "unitree_arm_sdk/utilities/timer.h" #include "utilities/timer.h"
namespace UNITREE_ARM { namespace UNITREE_ARM {
enum class BlockYN{ enum class BlockYN{

4
include/unitree_arm_sdk/model/ArmModel.h
View File

@ -2,8 +2,8 @@
#define ARMMODEL_H #define ARMMODEL_H
#include <vector> #include <vector>
#include "unitree_arm_sdk/thirdparty/robotics.h" #include "thirdparty/robotics.h"
#include "unitree_arm_sdk/thirdparty/quadProgpp/QuadProg++.hh" #include "thirdparty/quadProgpp/QuadProg++.hh"
namespace UNITREE_ARM { namespace UNITREE_ARM {

2
include/unitree_arm_sdk/thirdparty/quadProgpp/QuadProg++.hh vendored
View File

@ -62,7 +62,7 @@ s.t.
#ifndef _QUADPROGPP #ifndef _QUADPROGPP
#define _QUADPROGPP #define _QUADPROGPP
#include "unitree_arm_sdk/thirdparty/quadProgpp/Array.hh" #include "thirdparty/quadProgpp/Array.hh"
#include <eigen3/Eigen/Dense> #include <eigen3/Eigen/Dense>
namespace quadprogpp { namespace quadprogpp {

2
include/unitree_arm_sdk/thirdparty/robotics.h vendored
View File

@ -1,7 +1,7 @@
#pragma once #pragma once
#include <vector> #include <vector>
#include "unitree_arm_sdk/math/mathTools.h" #include "math/mathTools.h"
namespace robo { namespace robo {
/* /*

2
include/unitree_arm_sdk/utilities/loop.h
View File

@ -10,7 +10,7 @@
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/function.hpp> #include <boost/function.hpp>
#include <boost/bind.hpp> #include <boost/bind.hpp>
#include "unitree_arm_sdk/utilities/timer.h" #include "utilities/timer.h"
namespace UNITREE_ARM { namespace UNITREE_ARM {
typedef boost::function<void ()> Callback; typedef boost::function<void ()> Callback;

BIN
lib/libZ1_SDK_Linux64.so
View File

Binary file not shown.