add python-armModel
This commit is contained in:
parent
da1f87c0c4
commit
1fac4d8be3
|
@ -62,7 +62,7 @@ void Z1ARM::printState(){
|
|||
std::cout<<"tauState: "<<lowstate->getTau().transpose()<<std::endl;
|
||||
|
||||
std::cout<<"------ Endeffector Cartesian Posture ------"<<std::endl;
|
||||
std::cout<<"rx ry rz x y z"<<std::endl;
|
||||
std::cout<<"roll pitch yaw x y z"<<std::endl;
|
||||
std::cout<<lowstate->endPosture.transpose()<<std::endl;
|
||||
}
|
||||
|
||||
|
|
|
@ -14,11 +14,12 @@ int main(){
|
|||
for(int i=0; i<duration; i++)
|
||||
{
|
||||
arm.q = lastPos*(1-i/duration) + targetPos*(i/duration);
|
||||
arm.qd = (targetPos-lastPos)/(duration*0.002);
|
||||
arm.qd = (targetPos-lastPos)/(duration*arm._ctrlComp->dt);
|
||||
usleep(2000);
|
||||
}
|
||||
|
||||
arm.backToStart();
|
||||
arm.setFsm(UNITREE_ARM::ArmFSMState::PASSIVE);
|
||||
arm.sendRecvThread->shutdown();
|
||||
return 0;
|
||||
}
|
|
@ -23,6 +23,8 @@ public:
|
|||
|
||||
namespace py = pybind11;
|
||||
PYBIND11_MODULE(unitree_arm_interface, m){
|
||||
using rvp = py::return_value_policy;
|
||||
|
||||
py::enum_<ArmFSMState>(m, "ArmFSMState")
|
||||
.value("INVALID", ArmFSMState::INVALID)
|
||||
.value("PASSIVE", ArmFSMState::PASSIVE)
|
||||
|
@ -32,14 +34,47 @@ PYBIND11_MODULE(unitree_arm_interface, m){
|
|||
;
|
||||
|
||||
py::class_<LowlevelState>(m, "LowlevelState")
|
||||
.def("getQ", &LowlevelState::getQ, py::return_value_policy::reference_internal)
|
||||
.def("getQd", &LowlevelState::getQd, py::return_value_policy::reference_internal)
|
||||
.def("getQdd", &LowlevelState::getQdd, py::return_value_policy::reference_internal)
|
||||
.def("getQTau", &LowlevelState::getTau, py::return_value_policy::reference_internal)
|
||||
.def("getQ", &LowlevelState::getQ, rvp::reference_internal)
|
||||
.def("getQd", &LowlevelState::getQd, rvp::reference_internal)
|
||||
.def("getQdd", &LowlevelState::getQdd, rvp::reference_internal)
|
||||
.def("getQTau", &LowlevelState::getTau, rvp::reference_internal)
|
||||
;
|
||||
|
||||
py::class_<CtrlComponents>(m, "CtrlComponents")
|
||||
.def_readwrite("armModel", &CtrlComponents::armModel)
|
||||
.def_readonly("dt", &CtrlComponents::dt)
|
||||
;
|
||||
|
||||
py::class_<Z1Model>(m, "Z1Model")
|
||||
.def(py::init<Vec3, double, Vec3, Mat3>())
|
||||
.def("checkInSingularity", &Z1Model::checkInSingularity)
|
||||
// Pass-by-reference does not work in this method.
|
||||
// [incompatible function arguments.] Z1Model.jointProtect(arm.q, arm.qd)
|
||||
//.def("jointProtect", &Z1Model::jointProtect)
|
||||
.def("jointProtect", [](Z1Model& self, Vec6 q, Vec6 qd){
|
||||
self.jointProtect(q, qd);
|
||||
return std::make_pair(q, qd);
|
||||
})
|
||||
.def("getJointQMax", &Z1Model::getJointQMax, rvp::reference_internal)
|
||||
.def("getJointQMin", &Z1Model::getJointQMin, rvp::reference_internal)
|
||||
.def("getJointSpeedMax", &Z1Model::getJointSpeedMax, rvp::reference_internal)
|
||||
.def("inverseKinematics", [](Z1Model& self, HomoMat Tdes, Vec6 qPast, bool checkInWorkSpace){
|
||||
Vec6 q_result;
|
||||
bool hasIK = self.inverseKinematics(Tdes, qPast, q_result, checkInWorkSpace);
|
||||
return std::make_pair(hasIK, q_result);
|
||||
})
|
||||
.def("forwardKinematics", &Z1Model::forwardKinematics)
|
||||
.def("inverseDynamics", &Z1Model::inverseDynamics)
|
||||
.def("CalcJacobian", &Z1Model::CalcJacobian)
|
||||
.def("solveQP", [](Z1Model& self, Vec6 twist, Vec6 qPast, double dt){
|
||||
Vec6 qd_result;
|
||||
self.solveQP(twist, qPast, qd_result, dt);
|
||||
return qd_result;
|
||||
})
|
||||
;
|
||||
|
||||
py::class_<ArmInterface>(m, "ArmInterface")
|
||||
.def(py::init<bool>())
|
||||
.def(py::init<bool>(), py::arg("hasGripper")=true)
|
||||
.def_readwrite("q", &ArmInterface::q)
|
||||
.def_readwrite("qd", &ArmInterface::qd)
|
||||
.def_readwrite("tau", &ArmInterface::tau)
|
||||
|
|
|
@ -5,8 +5,7 @@ import time
|
|||
import numpy as np
|
||||
|
||||
np.set_printoptions(precision=3, suppress=True)
|
||||
hasGripper = True
|
||||
arm = unitree_arm_interface.ArmInterface(hasGripper)
|
||||
arm = unitree_arm_interface.ArmInterface(hasGripper=True)
|
||||
armState = unitree_arm_interface.ArmFSMState
|
||||
arm.loopOn()
|
||||
|
||||
|
@ -15,7 +14,7 @@ arm.labelRun("forward")
|
|||
arm.startTrack(armState.JOINTCTRL)
|
||||
for i in range(0, 1000):
|
||||
arm.jointCtrlCmd(np.array([0,0,0,-1,0,0,-1]), 0.5)
|
||||
time.sleep(0.002)
|
||||
time.sleep(arm._ctrlComp.dt)
|
||||
|
||||
# 2. highcmd_basic : armCtrlByFSM
|
||||
arm.labelRun("forward")
|
||||
|
@ -28,7 +27,7 @@ arm.labelRun("forward")
|
|||
arm.startTrack(armState.CARTESIAN)
|
||||
for i in range(0, 1000):
|
||||
arm.cartesianCtrlCmd(np.array([0,0,0,0,0,-1,-1]), 0.3, 0.1)
|
||||
time.sleep(0.002)
|
||||
time.sleep(arm._ctrlComp.dt)
|
||||
|
||||
arm.backToStart()
|
||||
arm.loopOff()
|
||||
|
|
|
@ -5,8 +5,8 @@ import time
|
|||
import numpy as np
|
||||
|
||||
np.set_printoptions(precision=3, suppress=True)
|
||||
hasGripper = True
|
||||
arm = unitree_arm_interface.ArmInterface(hasGripper)
|
||||
arm = unitree_arm_interface.ArmInterface(hasGripper=True)
|
||||
armModel = arm._ctrlComp.armModel
|
||||
arm.setFsmLowcmd()
|
||||
|
||||
duration = 1000
|
||||
|
@ -16,11 +16,11 @@ targetPos = np.array([0.0, 1.5, -1.0, -0.54, 0.0, 0.0]) #forward
|
|||
for i in range(0, duration):
|
||||
arm.q = lastPos*(1-i/duration) + targetPos*(i/duration)# set position
|
||||
arm.qd = (targetPos-lastPos)/(duration*0.002) # set velocity
|
||||
arm.tau = np.zeros(6) # set torque
|
||||
arm.tau = armModel.inverseDynamics(arm.q, arm.qd, np.zeros(6), np.zeros(6)) # set torque
|
||||
arm.gripperQ = -1*(i/duration)
|
||||
arm.sendRecv()# udp connection
|
||||
print(arm.lowstate.getQ())
|
||||
time.sleep(0.002)
|
||||
# print(arm.lowstate.getQ())
|
||||
time.sleep(arm._ctrlComp.dt)
|
||||
|
||||
arm.loopOn()
|
||||
arm.backToStart()
|
||||
|
|
|
@ -0,0 +1,40 @@
|
|||
import sys
|
||||
sys.path.append("../lib")
|
||||
import unitree_arm_interface
|
||||
import numpy as np
|
||||
|
||||
np.set_printoptions(precision=3, suppress=True)
|
||||
arm = unitree_arm_interface.ArmInterface(hasGripper = True)
|
||||
armModel = arm._ctrlComp.armModel
|
||||
|
||||
print('--------------------------FK & IK------------------------')
|
||||
q_FORWARD = np.array([0, 1.5, -1, 0.54, 0, 0])
|
||||
q_near_forward = np.array([0, 1.49, -1, 0.57, 0, 0])
|
||||
# 1. FK
|
||||
T_forward = armModel.forwardKinematics(q_FORWARD, 6)
|
||||
# 2. IK, q_result doesn't need to be near qPast
|
||||
hasIK, q_forward = armModel.inverseKinematics(T_forward, np.zeros(6), True)
|
||||
if hasIK:
|
||||
print("The joint angles corresponding to position FORWRAD:")
|
||||
print(q_forward)
|
||||
else:
|
||||
print("no IK")
|
||||
# 2. IK, q_result should to be near qPast
|
||||
hasIK, q_forward = armModel.inverseKinematics(T_forward, np.zeros(6), False)
|
||||
print(hasIK)
|
||||
hasIK, q_forward = armModel.inverseKinematics(T_forward, q_near_forward, False)
|
||||
print(hasIK, '\n')
|
||||
|
||||
print('--------------------------ID------------------------')
|
||||
tau = armModel.inverseDynamics(np.zeros(6), np.zeros(6), np.zeros(6), np.zeros(6))
|
||||
print("The torque required by the z1 arm at the homo position to resist gravity:")
|
||||
print(tau, '\n')
|
||||
|
||||
print('--------------------------jacobian------------------------')
|
||||
print("compute: V = J * qd")
|
||||
J = armModel.CalcJacobian(q_FORWARD)
|
||||
spatialtwist = np.array([0, 0, 0, 1., 0, 0])
|
||||
qd = np.linalg.inv(J).dot(spatialtwist)
|
||||
print("solved by jacobian: ", qd)
|
||||
qd = armModel.solveQP(spatialtwist, q_near_forward, arm._ctrlComp.dt)
|
||||
print("solved by QP: ", qd)
|
|
@ -13,8 +13,6 @@ struct CtrlComponents{
|
|||
public:
|
||||
CtrlComponents(double deltaT, bool hasUnitreeGripper);
|
||||
~CtrlComponents();
|
||||
|
||||
|
||||
/*
|
||||
* Function: send udp message to z1_ctrl and receive udp message from it
|
||||
* Input: None
|
||||
|
@ -23,8 +21,6 @@ public:
|
|||
* and call udp->recv() to store datas from z1_ctrl into lowstate
|
||||
*/
|
||||
void sendRecv();
|
||||
|
||||
|
||||
/*
|
||||
* Function: Set six joints commands to class lowcmd
|
||||
* Input: q: joint angle
|
||||
|
@ -33,8 +29,6 @@ public:
|
|||
* Output: None
|
||||
*/
|
||||
void armCtrl(Vec6 q, Vec6 qd, Vec6 tau);
|
||||
|
||||
|
||||
/*
|
||||
* Function: Set gripper commands to class lowcmd
|
||||
* Input: q: joint angle
|
||||
|
|
|
@ -10,6 +10,7 @@ public:
|
|||
unitreeArm(bool hasUnitreeGripper);
|
||||
~unitreeArm();
|
||||
|
||||
|
||||
/*
|
||||
* Function: Change z1_ctrl state to fsm, wait until change complete
|
||||
* Input: ArmFSMState
|
||||
|
@ -81,7 +82,7 @@ void calibration();
|
|||
|
||||
/*
|
||||
* Function: Move the robot in a joint path
|
||||
* Input: posture: target position, (rx ry rz x y z), unit: meter
|
||||
* Input: posture: target position, (roll pitch yaw x y z), unit: meter
|
||||
* maxSpeed: the maximum joint speed when robot is moving, unit: radian/s
|
||||
* range:[0, pi]
|
||||
* Output: None
|
||||
|
@ -91,7 +92,7 @@ bool MoveJ(Vec6 posture, double maxSpeed);
|
|||
|
||||
/*
|
||||
* Function: Move the robot in a joint path, and control the gripper at the same time
|
||||
* Input: posture: target position, (rx ry rz x y z), unit: meter
|
||||
* Input: posture: target position, (roll pitch yaw x y z), unit: meter
|
||||
* gripperPos: target angular
|
||||
* uint: radian
|
||||
* range:[-pi/2, 0]
|
||||
|
@ -105,7 +106,7 @@ bool MoveJ(Vec6 posture, double gripperPos, double maxSpeed);
|
|||
|
||||
/*
|
||||
* Function: Move the robot in a linear path
|
||||
* Input: posture: target position, (rx ry rz x y z), unit: meter
|
||||
* Input: posture: target position, (roll pitch yaw x y z), unit: meter
|
||||
* maxSpeed: the maximum joint speed when robot is moving, unit: m/s
|
||||
* Output: whether posture has inverse kinematics
|
||||
*/
|
||||
|
@ -114,7 +115,7 @@ bool MoveL(Vec6 posture, double maxSpeed);
|
|||
|
||||
/*
|
||||
* Function: Move the robot in a linear path, and control the gripper at the same time
|
||||
* Input: posture: target position, (rx ry rz x y z), unit: meter
|
||||
* Input: posture: target position, (roll pitch yaw x y z), unit: meter
|
||||
* gripperPos: target angular, uint: radian
|
||||
* range:[-pi/2, 0]
|
||||
* maxSpeed: the maximum joint speed when robot is moving, unit: m/s
|
||||
|
@ -126,7 +127,7 @@ bool MoveL(Vec6 posture, double gripperPos, double maxSpeed);
|
|||
/*
|
||||
* Function: Move the robot in a circular path
|
||||
* Input: middle posture: determine the shape of the circular path
|
||||
* endPosture: target position, (rx ry rz x y z), unit: meter
|
||||
* endPosture: target position, (roll pitch yaw x y z), unit: meter
|
||||
* maxSpeed: the maximum joint speed when robot is moving, unit: m/s
|
||||
* Output: whether posture has inverse kinematics
|
||||
*/
|
||||
|
@ -136,7 +137,7 @@ bool MoveC(Vec6 middlePosutre, Vec6 endPosture, double maxSpeed);
|
|||
/*
|
||||
* Function: Move the robot in a circular path, and control the gripper at the same time
|
||||
* Input: middle posture: determine the shape of the circular path
|
||||
* endPosture: target position, (rx ry rz x y z), unit: meter
|
||||
* endPosture: target position, (roll pitch yaw x y z), unit: meter
|
||||
* gripperPos: target angular, uint: radian
|
||||
* range:[-pi/2, 0]
|
||||
* maxSpeed: the maximum joint speed when robot is moving, unit: m/s
|
||||
|
@ -220,7 +221,7 @@ void jointCtrlCmd(Vec7 directions, double jointSpeed);
|
|||
/*
|
||||
* Function: set spatial velocity command automatically by input parameters
|
||||
* Input: directions: movement directions [include gripper], range:[-1,1]
|
||||
* rx, ry, rz, x, y, z, gripper
|
||||
* roll, pitch, yaw, x, y, z, gripper
|
||||
* oriSpeed: range: [0, 0.6]
|
||||
* posSpeed: range: [0, 0.3]
|
||||
* gripper joint speed is set to 1.0
|
||||
|
|
|
@ -12,6 +12,8 @@ class ArmModel{
|
|||
public:
|
||||
ArmModel(Vec3 endPosLocal, double endEffectorMass, Vec3 endEffectorCom, Mat3 endEffectorInertia);
|
||||
~ArmModel(){};
|
||||
|
||||
|
||||
/*
|
||||
* Function: compute end effector frame (used for current spatial position calculation)
|
||||
* Inputs: q: current joint angles
|
||||
|
@ -41,7 +43,7 @@ HomoMat forwardKinematics(Vec6 q, int index = 6);
|
|||
* number of maximum iterations without finding a solution
|
||||
* q_result: Joint angles that achieve T within the specified tolerances,
|
||||
*/
|
||||
virtual bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWorkSpace = false);
|
||||
virtual bool inverseKinematics(HomoMat TDes, Vec6 qPast, Eigen::Ref<Vec6> q_result, bool checkInWorkSpace = false);
|
||||
|
||||
|
||||
/*
|
||||
|
@ -61,7 +63,7 @@ Mat6 CalcJacobian(Vec6 q);
|
|||
* Returns: required joint forces/torques
|
||||
*/
|
||||
Vec6 inverseDynamics(Vec6 q, Vec6 qd, Vec6 qdd, Vec6 Ftip);
|
||||
virtual void solveQP(Vec6 twist, Vec6 qPast, Vec6& qd_result, double dt) = 0;
|
||||
virtual void solveQP(Vec6 twist, Vec6 qPast, Eigen::Ref<Vec6> qd_result, double dt) = 0;
|
||||
virtual bool checkInSingularity(Vec6 q) = 0;
|
||||
|
||||
|
||||
|
@ -71,7 +73,7 @@ virtual bool checkInSingularity(Vec6 q) = 0;
|
|||
* qd: set in range[-_jointSpeedMax, _jointSpeedMax]
|
||||
* Returns: None
|
||||
*/
|
||||
void jointProtect(Vec6& q, Vec6& qd);
|
||||
void jointProtect(Eigen::Ref<Vec6> q, Eigen::Ref<Vec6> qd);
|
||||
std::vector<double> getJointQMax() {return _jointQMax;}
|
||||
std::vector<double> getJointQMin() {return _jointQMin;}
|
||||
std::vector<double> getJointSpeedMax() {return _jointSpeedMax;}
|
||||
|
@ -120,6 +122,8 @@ public:
|
|||
Z1Model(Vec3 endPosLocal = Vec3::Zero(), double endEffectorMass = 0.0,
|
||||
Vec3 endEffectorCom = Vec3::Zero(), Mat3 endEffectorInertia = Mat3::Zero());
|
||||
~Z1Model(){};
|
||||
|
||||
|
||||
/*
|
||||
* Function: Check whether joint1 and joint5 is coaxial
|
||||
* x5^2 + y5^2 < 0.1^2
|
||||
|
@ -146,7 +150,7 @@ bool checkInSingularity(Vec6 q);
|
|||
* number of maximum iterations without finding a solution
|
||||
* q_result: Joint angles that achieve T within the specified tolerances,
|
||||
*/
|
||||
bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWorkSpace = false);
|
||||
bool inverseKinematics(HomoMat TDes, Vec6 qPast, Eigen::Ref<Vec6> q_result, bool checkInWorkSpace = false);
|
||||
|
||||
|
||||
/*
|
||||
|
@ -156,7 +160,7 @@ bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWor
|
|||
* dt : compute period
|
||||
* Returns: qd_result: joint velocity that are corresponding to twist
|
||||
*/
|
||||
void solveQP(Vec6 twist, Vec6 qPast, Vec6& qd_result, double dt);
|
||||
void solveQP(Vec6 twist, Vec6 qPast, Eigen::Ref<Vec6> qd_result, double dt);
|
||||
|
||||
|
||||
private:
|
||||
|
|
Binary file not shown.
Loading…
Reference in New Issue