Skip to content

Commit

Permalink
fix: 修正move_pvat不能正常工作的问题
Browse files Browse the repository at this point in the history
  • Loading branch information
@liufang-robot
liufang-robot committed Nov 21, 2024
1 parent d0966ed commit d226010
Show file tree
Hide file tree
Showing 9 changed files with 112 additions and 54 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@ endif()

project(
lebai
VERSION 1.1.28
VERSION 1.1.29
LANGUAGES CXX)

list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
Expand Down
2 changes: 1 addition & 1 deletion Doxyfile
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ PROJECT_NAME = "lebai sdk"
# could be handy for archiving the generated documentation or if some version
# control system is used.

PROJECT_NUMBER = 1.1.28
PROJECT_NUMBER = 1.1.29

# Using the PROJECT_BRIEF tag one can provide an optional one line description
# for a project that appears at the top of each page and should give viewer a
Expand Down
3 changes: 3 additions & 0 deletions doc/changelog.md
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,8 @@
# ChangeLog

## 1.1.29
修正move_pvat不能正常工作的问题。

## 1.1.28
修正c++安装包CMake Config的错误，添加使用文档。

Expand Down
2 changes: 1 addition & 1 deletion doc/faq.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ FAQ
# 机械臂界面上坐标系统中，Rz、Ry、Rx是如何定义的。
乐白机械臂界面的Rz,Rz,Rz表示机械臂姿态，是由EulerZYX确定的。
乐白机械臂界面的Rx,Ry,Rz表示机械臂姿态，是由EulerZYX确定的。
界面显示的Rz，Ry，Rx为角度，将其转换为弧度后，它和旋转矩阵的关系如下：
Expand Down
115 changes: 76 additions & 39 deletions examples/example.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,22 +20,18 @@
#include <cmath>
#include <lebai/robot.hh>

int main(int argc, char **argv)
{
if (argc < 2)
{
int main(int argc, char **argv) {
if (argc < 2) {
std::cerr << "You must specify the IP address of the robot" << std::endl;
std::cerr << "Execute example as follow:" << std::endl;
std::cerr << "./example 192.168.1.200 sim" << std::endl;
return 0;
}
std::string ip = argv[1];
bool sim = false;
if (argc > 2)
{
if (argc > 2) {
std::string sim_str = argv[2];
if (sim_str == "sim")
{
if (sim_str == "sim") {
sim = true;
}
}
Expand All @@ -44,23 +40,51 @@ int main(int argc, char **argv)
// Create robot instance
lebai::l_master::Robot robot(ip, sim);
std::this_thread::sleep_for(std::chrono::seconds(1));
robot.movej({13.0 / 180.0 * M_PI, -52.0 / 180.0 * M_PI, 86.0 / 180.0 * M_PI, 8.0 / 180.0 * M_PI, -59.0 / 180.0 * M_PI, -11.0 / 180.0 * M_PI}, 1.0, 0.5, 0.0, 0.0);
robot.movej({0.0 / 180.0 * M_PI, -52.0 / 180.0 * M_PI, 86.0 / 180.0 * M_PI,
8.0 / 180.0 * M_PI, -59.0 / 180.0 * M_PI, -11.0 / 180.0 * M_PI},
1.0, 0.5, 0.0, 0.0);
robot.movej({0.0, 0.0, 0.0, 0.0, 0.0, 0.5}, 1.0, 0.5, 0.0, 0.0);
robot.movej({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 1.0, 0.5, 0.0, 0.0);

// robot.movej({0.0 / 180.0 * M_PI, -52.0 / 180.0 * M_PI, 86.0 / 180.0 * M_PI,
// 8.0 / 180.0 * M_PI, -59.0 / 180.0 * M_PI, -11.0 / 180.0 *
// M_PI},
// 1.0, 0.5, 0.0, 0.0);
robot.wait_move();
for (size_t i = 0; i < 30; ++i)
{
std::this_thread::sleep_for(std::chrono::seconds(1));
try {
robot.get_robot_mode();
}
catch(std::runtime_error & ex)
{
std::cout<<"get_robot_mode error: "<<ex.what()<<std::endl;
}

}
// movej({j1=0,j2=0,j3=0,j4=0,j5=0,j6=0}, 0.1, 0.1, 0, 0)
// move_pvat({j1=0.1,j2=0,j3=0,j4=0,j5=0,j6=0}, 0.1, 0.1, 2)
// move_pvat({j1=0.2,j2=0,j3=0,j4=0,j5=0,j6=0}, 0.1, 0.1, 2)
// move_pvat({j1=0.3,j2=0,j3=0,j4=0,j5=0,j6=0}, 0.1, 0.1, 2)
// move_pvat({j1=0.4,j2=0,j3=0,j4=0,j5=0,j6=0}, 0.1, 0.1, 2)
robot.move_pvat({0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1}, 2.0);
std::this_thread::sleep_for(std::chrono::milliseconds(500));
robot.move_pvat({0.2, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1}, 2.0);
std::this_thread::sleep_for(std::chrono::milliseconds(500));
robot.move_pvat({0.3, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1}, 2.0);
std::this_thread::sleep_for(std::chrono::milliseconds(500));
robot.move_pvat({0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1}, 2.0);
std::this_thread::sleep_for(std::chrono::milliseconds(500));
robot.move_pvat({0.5, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1}, 2.0);
robot.wait_move();
// for (size_t i = 0; i < 30; ++i)
// {
// std::this_thread::sleep_for(std::chrono::seconds(1));
// try {
// robot.get_robot_mode();
// }
// catch(std::runtime_error & ex)
// {
// std::cout<<"get_robot_mode error: "<<ex.what()<<std::endl;
// }

// }

// std::cout << "after sleep" << std::endl;
// bool is_network_connected = robot.is_network_connected();
Expand All @@ -73,24 +97,37 @@ int main(int argc, char **argv)
// }

// robot.wait_move();
// robot.movec({{"x", -0.282541}, {"y", -0.168246}, {"z", 0.265824}, {"rz", 1.27256}, {"ry", -0.206353}, {"rx", 0.937445}},
// {{"x", -0.255832}, {"y", 0.00270435}, {"z", 0.266642}, {"rz", 1.27293}, {"ry", -0.20805}, {"rx", 0.94485}},
// robot.movec({{"x", -0.282541}, {"y", -0.168246}, {"z", 0.265824},
// {"rz", 1.27256}, {"ry", -0.206353}, {"rx", 0.937445}},
// {{"x", -0.255832}, {"y", 0.00270435}, {"z", 0.266642}, {"rz", 1.27293},
// {"ry", -0.20805}, {"rx", 0.94485}},
// 0.0, 1.0, 0.5, 0.0, 0.0);
// kinematics_forward: -0.282541, -0.168246, 0.265824, 1.27256, -0.206353, 0.937445
// kinematics_forward: -0.255832, 0.00270435, 0.266642, 1.27293, -0.20805, 0.94485
// kinematics_forward: -0.282541, -0.168246, 0.265824, 1.27256, -0.206353,
// 0.937445 kinematics_forward: -0.255832, 0.00270435, 0.266642, 1.27293,
// -0.20805, 0.94485

// auto fk_resp = robot.kinematics_forward({3.0/ 180.0 * M_PI, -48.0/ 180.0 * M_PI, 78.0/ 180.0 * M_PI, 9.0/ 180.0 * M_PI, -67.0/ 180.0 * M_PI, -3.0/ 180.0 * M_PI});
// std::vector<double> jp = {3.0/ 180.0 * M_PI, -48.0/ 180.0 * M_PI, 78.0/ 180.0 * M_PI, 9.0/ 180.0 * M_PI, -67.0/ 180.0 * M_PI, -3.0/ 180.0 * M_PI};
// std::cout<<"jp "<<jp[0]<<", "<<jp[1]<<", "<<jp[2]<<", "<<jp[3]<<", "<<jp[4]<<", "<<jp[5]<<std::endl;
// std::cout<<"kinematics_forward: "<<fk_resp.pose["x"]<<", "<<fk_resp.pose["y"]<<", "<<fk_resp.pose["z"]<<", "<<fk_resp.pose["rx"]<<", "<<fk_resp.pose["ry"]<<", "<<fk_resp.pose["rz"]<<std::endl;
// auto ik_resp = robot.kinematics_inverse(fk_resp.pose);
// if(ik_resp.ok)
// auto fk_resp = robot.kinematics_forward({3.0/ 180.0 * M_PI, -48.0/ 180.0 *
// M_PI, 78.0/ 180.0 * M_PI, 9.0/ 180.0 * M_PI, -67.0/ 180.0 * M_PI, -3.0/
// 180.0 * M_PI}); std::vector<double> jp = {3.0/ 180.0 * M_PI, -48.0/ 180.0 *
// M_PI, 78.0/ 180.0 * M_PI, 9.0/ 180.0 * M_PI, -67.0/ 180.0 * M_PI, -3.0/
// 180.0 * M_PI}; std::cout<<"jp "<<jp[0]<<", "<<jp[1]<<", "<<jp[2]<<",
// "<<jp[3]<<", "<<jp[4]<<", "<<jp[5]<<std::endl;
// std::cout<<"kinematics_forward: "<<fk_resp.pose["x"]<<",
// "<<fk_resp.pose["y"]<<", "<<fk_resp.pose["z"]<<", "<<fk_resp.pose["rx"]<<",
// "<<fk_resp.pose["ry"]<<", "<<fk_resp.pose["rz"]<<std::endl; auto ik_resp =
// robot.kinematics_inverse(fk_resp.pose); if(ik_resp.ok)
//{
// std::cout<<"kinematics_inverse: "<<ik_resp.joint_positions[0]<<", "<<ik_resp.joint_positions[1]<<", "<<ik_resp.joint_positions[2]<<", "<<ik_resp.joint_positions[3]<<", "<<ik_resp.joint_positions[4]<<", "<<ik_resp.joint_positions[5]<<std::endl;
// std::cout<<"kinematics_inverse: "<<ik_resp.joint_positions[0]<<",
// "<<ik_resp.joint_positions[1]<<", "<<ik_resp.joint_positions[2]<<",
// "<<ik_resp.joint_positions[3]<<", "<<ik_resp.joint_positions[4]<<",
// "<<ik_resp.joint_positions[5]<<std::endl;
// }
// jp = {-28/ 180.0 * M_PI, -59.0/ 180.0 * M_PI, 96.0/ 180.0 * M_PI, -2.0/ 180.0 * M_PI, -92.0/ 180.0 * M_PI, 16.0/ 180.0 * M_PI};
// std::cout<<"jp "<<jp[0]<<", "<<jp[1]<<", "<<jp[2]<<", "<<jp[3]<<", "<<jp[4]<<", "<<jp[5]<<std::endl;
// fk_resp = robot.kinematics_forward(jp);
// std::cout<<"kinematics_forward: "<<fk_resp.pose["x"]<<", "<<fk_resp.pose["y"]<<", "<<fk_resp.pose["z"]<<", "<<fk_resp.pose["rx"]<<", "<<fk_resp.pose["ry"]<<", "<<fk_resp.pose["rz"]<<std::endl;
// jp = {-28/ 180.0 * M_PI, -59.0/ 180.0 * M_PI, 96.0/ 180.0 * M_PI, -2.0/
// 180.0 * M_PI, -92.0/ 180.0 * M_PI, 16.0/ 180.0 * M_PI}; std::cout<<"jp
// "<<jp[0]<<", "<<jp[1]<<", "<<jp[2]<<", "<<jp[3]<<", "<<jp[4]<<",
// "<<jp[5]<<std::endl; fk_resp = robot.kinematics_forward(jp);
// std::cout<<"kinematics_forward: "<<fk_resp.pose["x"]<<",
// "<<fk_resp.pose["y"]<<", "<<fk_resp.pose["z"]<<", "<<fk_resp.pose["rx"]<<",
// "<<fk_resp.pose["ry"]<<", "<<fk_resp.pose["rz"]<<std::endl;
return 0;
}
9 changes: 5 additions & 4 deletions sdk/include/lebai/robot.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,14 +102,14 @@ class Robot {
*
*/
virtual ~Robot();

// clang-format off
/**
* 示例代码:
*
* std::string movej_req =
* "{\"param\":{\"v\":0.1},\"pose\":{\"joint\":{\"delta\":"{\"joint\":[-1.0,0.0,0.0,0.0,0.0,0.0]}}}}";
* std::string movej_req = "{\"param\":{\"v\":0.1},\"pose\":{\"joint\":{\"delta\":"{\"joint\":[-1.0,0.0,0.0,0.0,0.0,0.0]}}}}";
* resp = robot.call("movej", movej_req);
* std::cout << "resp: " << std::get<0>(resp) << ", " << std::get<1>(resp)
* << std::endl;
* std::cout << "resp: " << std::get<0>(resp) << ", " << std::get<1>(resp) << std::endl;
*
*
* @brief 用JSON格式字符串调用机械臂的接口.
Expand All @@ -121,6 +121,7 @@ class Robot {
* 如果返回码为0，表示调用成功，第二个元素是JSONRPC的返回数据.
* 如果返回码为非0，表示调用失败，第二个元素是错误信息.
*/
// clang-format on
std::tuple<int, std::string> call(const std::string &method,
const std::string &params);

Expand Down
9 changes: 5 additions & 4 deletions sdk/src/protos/motion.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -322,13 +322,13 @@ const double JointMove::acc() const { return acc_; }
double* JointMove::mutable_acc() { return &acc_; }
bool JointMove::Deserialize(const rapidjson::Value& obj) {
if (obj.HasMember("pose")) {
pose_ = (double)(obj["pose"].GetDouble());
pose_ = obj["pose"].GetDouble();
}
if (obj.HasMember("velocity")) {
velocity_ = (double)(obj["velocity"].GetDouble());
velocity_ = obj["velocity"].GetDouble();
}
if (obj.HasMember("acc")) {
acc_ = (double)(obj["acc"].GetDouble());
acc_ = obj["acc"].GetDouble();
}
return true;
}
Expand Down Expand Up @@ -374,9 +374,10 @@ bool MovePvatRequest::Serialize(
writer->StartObject();
writer->Key("duration");
writer->Double(duration_);
writer->Key("joints");
writer->StartArray();
for (auto j : joints_) {
writer->String(j.ToJSONString().c_str());
j.Serialize(writer);
}
writer->EndArray();
writer->EndObject();
Expand Down
6 changes: 2 additions & 4 deletions sdk/src/robot_impl.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -141,21 +141,19 @@ motion::MotionIndex Robot::RobotImpl::speedLinear(
std::string resp;

auto req_string = req.ToJSONString();
std::cout << "req " << req_string << "\n";
json_rpc_connector_->CallRpc("speed_linear", req_string, &resp);
// json_rpc_connector_->CallRpc("speed_linear",req.ToJSONString(),&resp);
motion::MotionIndex motion_resp;
motion_resp.FromJSONString(resp);
std::cout << "resp " << resp << "\n";
return motion_resp;
}
void Robot::RobotImpl::movePvat(const motion::MovePvatRequest &req) {
json_rpc_connector_->CallRpc("move_pvat", req.ToJSONString(), nullptr);
std::string resp;
json_rpc_connector_->CallRpc("move_pvat", req.ToJSONString(), &resp);
}
void Robot::RobotImpl::waitMove(const motion::MotionIndex &req) {
std::string resp;
json_rpc_connector_->CallRpc("wait_move", req.ToJSONString(), &resp);
// std::cout<<"resp " << resp << std::endl;
}
motion::MotionIndex Robot::RobotImpl::getRunningMotion() {
std::string resp;
Expand Down
18 changes: 18 additions & 0 deletions sdk/test/test_protos.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -378,6 +378,24 @@ TEST_F(ProtosTest, TestMotion) {
EXPECT_NEAR(0.3, *req_check.mutable_param()->mutable_velocity(), 1e-6);
EXPECT_NEAR(0.5, *req_check.mutable_rad(), 1e-6);
}
{
lebai::motion::JointMove joint_move;
joint_move.set_pose(0.1);
joint_move.set_velocity(0.2);
joint_move.set_acc(0.3);
auto json_str = joint_move.ToJSONString();
// "{"pose":0.1,"velocity":0.2,"acc":0.3}"
EXPECT_EQ("{\"pose\":0.1,\"velocity\":0.2,\"acc\":0.3}", json_str);
lebai::motion::MovePvatRequest req;
req.set_duration(0.4);
req.set_joints({joint_move});
json_str = req.ToJSONString();
// {"duration":0.4,"joints":[{"pose":0.1,"velocity":0.2,"acc":0.3}]}
EXPECT_EQ(
"{\"duration\":0.4,\"joints\":[{\"pose\":0.1,\"velocity\":0.2,\"acc\":"
"0.3}]}",
json_str);
}
}
} // namespace lebai

Expand Down

0 comments on commit d226010

Please sign in to comment.