添加了gou2这个功能包，该功能包用于gazebo仿真
刘志通
parent
5f068c763c
commit
d5a39c6425
|
|
@ -0,0 +1,45 @@
|
||||||
|
cmake_minimum_required(VERSION 2.8.3)
|
||||||
|
|
||||||
|
project(gou2)
|
||||||
|
|
||||||
|
find_package(catkin REQUIRED)
|
||||||
|
|
||||||
|
catkin_package()
|
||||||
|
|
||||||
|
find_package(roslaunch)
|
||||||
|
|
||||||
|
find_package(catkin REQUIRED COMPONENTS
|
||||||
|
controller_manager
|
||||||
|
gazebo_ros
|
||||||
|
gazebo_ros_control
|
||||||
|
joint_state_publisher
|
||||||
|
robot_state_publisher
|
||||||
|
roscpp
|
||||||
|
rospy
|
||||||
|
rviz
|
||||||
|
xacro
|
||||||
|
kdl_parser
|
||||||
|
tf
|
||||||
|
sensor_msgs
|
||||||
|
std_msgs
|
||||||
|
trac_ik_lib
|
||||||
|
serial
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
include_directories(
|
||||||
|
include
|
||||||
|
${catkin_INCLUDE_DIRS}
|
||||||
|
${orocos_kdl_INCLUDE_DIRS}
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
add_executable(gou2 src/gou2.cpp)
|
||||||
|
target_link_libraries(gou2 ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES})
|
||||||
|
|
||||||
|
foreach(dir config launch meshes urdf)
|
||||||
|
install(DIRECTORY ${dir}/
|
||||||
|
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
|
||||||
|
endforeach(dir)
|
||||||
|
|
@ -0,0 +1,54 @@
|
||||||
|
gou2:
|
||||||
|
# Publish all joint states -----------------------------------
|
||||||
|
joint_state_controller:
|
||||||
|
type: joint_state_controller/JointStateController
|
||||||
|
publish_rate: 50
|
||||||
|
|
||||||
|
# Position Controllers ---------------------------------------
|
||||||
|
lf_zhou_con:
|
||||||
|
type: position_controllers/JointPositionController
|
||||||
|
joint: LF_ZHOU_JOINT
|
||||||
|
#pid: {p: 10.0, i: 0.01, d: 0.0}
|
||||||
|
lf_xi_con:
|
||||||
|
type: position_controllers/JointPositionController
|
||||||
|
joint: LF_XI_JOINT
|
||||||
|
#pid: {p: 10.0, i: 0.01, d: 0.0}
|
||||||
|
lb_zhou_con:
|
||||||
|
type: position_controllers/JointPositionController
|
||||||
|
joint: LB_ZHOU_JOINT
|
||||||
|
#pid: {p: 10.0, i: 0.01, d: 0.0}
|
||||||
|
lb_xi_con:
|
||||||
|
type: position_controllers/JointPositionController
|
||||||
|
joint: LB_XI_JOINT
|
||||||
|
#pid: {p: 10.0, i: 0.01, d: 0.0}
|
||||||
|
rf_zhou_con:
|
||||||
|
type: position_controllers/JointPositionController
|
||||||
|
joint: RF_ZHOU_JOINT
|
||||||
|
#pid: {p: 10.0, i: 0.01, d: 0.0}
|
||||||
|
rf_xi_con:
|
||||||
|
type: position_controllers/JointPositionController
|
||||||
|
joint: RF_XI_JOINT
|
||||||
|
#pid: {p: 10.0, i: 0.01, d: 0.0}
|
||||||
|
rb_zhou_con:
|
||||||
|
type: position_controllers/JointPositionController
|
||||||
|
joint: RB_ZHOU_JOINT
|
||||||
|
#pid: {p: 10.0, i: 0.01, d: 0.0}
|
||||||
|
rb_xi_con:
|
||||||
|
type: position_controllers/JointPositionController
|
||||||
|
joint: RB_XI_JOINT
|
||||||
|
#pid: {p: 10.0, i: 0.01, d: 0.0}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
gou2/gazebo_ros_control/pid_gains:
|
||||||
|
LF_ZHOU_JOINT: {p: 10.0, i: 0.0, d: 0.0}
|
||||||
|
LF_XI_JOINT: {p: 10.0, i: 0.0, d: 0.0}
|
||||||
|
LB_ZHOU_JOINT: {p: 10.0, i: 0.0, d: 0.0}
|
||||||
|
LB_XI_JOINT: {p: 10.0, i: 0.0, d: 0.0}
|
||||||
|
RF_ZHOU_JOINT: {p: 10.0, i: 0.0, d: 0.0}
|
||||||
|
RF_XI_JOINT: {p: 10.0, i: 0.0, d: 0.0}
|
||||||
|
RB_ZHOU_JOINT: {p: 10.0, i: 0.0, d: 0.0}
|
||||||
|
RB_XI_JOINT: {p: 10.0, i: 0.0, d: 0.0}
|
||||||
|
|
||||||
|
|
||||||
File diff suppressed because one or more lines are too long
|
|
@ -0,0 +1,26 @@
|
||||||
|
<launch>
|
||||||
|
<arg
|
||||||
|
name="model" />
|
||||||
|
<arg
|
||||||
|
name="gui"
|
||||||
|
default="true" />
|
||||||
|
<param
|
||||||
|
name="robot_description"
|
||||||
|
textfile="$(find gou2)/urdf/gou2.urdf" />
|
||||||
|
<param
|
||||||
|
name="use_gui"
|
||||||
|
value="$(arg gui)" />
|
||||||
|
<node
|
||||||
|
name="joint_state_publisher"
|
||||||
|
pkg="joint_state_publisher"
|
||||||
|
type="joint_state_publisher" />
|
||||||
|
<node
|
||||||
|
name="robot_state_publisher"
|
||||||
|
pkg="robot_state_publisher"
|
||||||
|
type="state_publisher" />
|
||||||
|
<node
|
||||||
|
name="rviz"
|
||||||
|
pkg="rviz"
|
||||||
|
type="rviz"
|
||||||
|
args="-d $(find gou2)/urdf.rviz" />
|
||||||
|
</launch>
|
||||||
|
|
@ -0,0 +1,39 @@
|
||||||
|
<launch>
|
||||||
|
<param name="robot_description" textfile="$(find gou2)/urdf/gou2.urdf" />
|
||||||
|
<rosparam file="$(find gou2)/config/gou2.yaml" command="load"/>
|
||||||
|
|
||||||
|
<!-- these are the arguments you can pass this launch file, for example paused:=true -->
|
||||||
|
<arg name="paused" default="false"/>
|
||||||
|
<arg name="use_sim_time" default="true"/>
|
||||||
|
<arg name="gui" default="true"/>
|
||||||
|
<arg name="headless" default="false"/>
|
||||||
|
<arg name="debug" default="false"/>
|
||||||
|
|
||||||
|
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
|
||||||
|
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||||
|
<arg name="world_name" value="$(find gou2)/worlds/gou2.world"/>
|
||||||
|
<arg name="debug" value="$(arg debug)" />
|
||||||
|
<arg name="gui" value="$(arg gui)" />
|
||||||
|
<arg name="paused" value="$(arg paused)"/>
|
||||||
|
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
|
||||||
|
<arg name="headless" value="$(arg headless)"/>
|
||||||
|
</include>
|
||||||
|
|
||||||
|
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" args="-urdf -model my_gou2 -param robot_description"/>
|
||||||
|
|
||||||
|
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
|
||||||
|
output="screen" ns="/gou2" args="lf_zhou_con
|
||||||
|
lf_xi_con
|
||||||
|
lb_zhou_con
|
||||||
|
lb_xi_con
|
||||||
|
rf_zhou_con
|
||||||
|
rf_xi_con
|
||||||
|
rb_zhou_con
|
||||||
|
rb_xi_con"/>
|
||||||
|
|
||||||
|
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
|
||||||
|
respawn="false" output="screen">
|
||||||
|
<remap from="/joint_states" to="/gou2/joint_states" />
|
||||||
|
</node>
|
||||||
|
|
||||||
|
</launch>
|
||||||
|
|
@ -0,0 +1,17 @@
|
||||||
|
<launch>
|
||||||
|
<arg name="gui" default="False" />
|
||||||
|
|
||||||
|
<param name="use_gui" value="$(arg gui)" />
|
||||||
|
|
||||||
|
<arg name="rvizconfig" default="$(find gou2)/rviz/urdf.rviz" />
|
||||||
|
|
||||||
|
<param name="robot1_description" textfile="$(find gou2)/urdf/gou2_ik.urdf" />
|
||||||
|
|
||||||
|
<node name="gou2" pkg="gou2" type="gou2" output="screen"/>
|
||||||
|
|
||||||
|
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
|
||||||
|
|
||||||
|
|
||||||
|
</launch>
|
||||||
|
|
||||||
|
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -0,0 +1,21 @@
|
||||||
|
<package format="2">
|
||||||
|
<name>gou2</name>
|
||||||
|
<version>1.0.0</version>
|
||||||
|
<description>
|
||||||
|
<p>URDF Description package for gou2</p>
|
||||||
|
<p>This package contains configuration data, 3D models and launch files
|
||||||
|
for gou2 robot</p>
|
||||||
|
</description>
|
||||||
|
<author>TODO</author>
|
||||||
|
<maintainer email="TODO@email.com" />
|
||||||
|
<license>BSD</license>
|
||||||
|
<buildtool_depend>catkin</buildtool_depend>
|
||||||
|
<depend>roslaunch</depend>
|
||||||
|
<depend>robot_state_publisher</depend>
|
||||||
|
<depend>rviz</depend>
|
||||||
|
<depend>joint_state_publisher</depend>
|
||||||
|
<depend>gazebo</depend>
|
||||||
|
<export>
|
||||||
|
<architecture_independent />
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
|
|
@ -0,0 +1,687 @@
|
||||||
|
#include <iostream>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include "ros/ros.h"
|
||||||
|
#include "sensor_msgs/JointState.h"
|
||||||
|
#include "turtlesim/Pose.h"
|
||||||
|
#include "tf2_ros/static_transform_broadcaster.h"
|
||||||
|
#include "tf2_ros/transform_broadcaster.h"
|
||||||
|
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
|
||||||
|
#include "geometry_msgs/TransformStamped.h"
|
||||||
|
#include "tf2/LinearMath/Quaternion.h"
|
||||||
|
#include "kdl_parser/kdl_parser.hpp"
|
||||||
|
#include "kdl/chainiksolverpos_nr_jl.hpp"
|
||||||
|
#include "trac_ik/trac_ik.hpp"
|
||||||
|
#include "urdf/model.h"
|
||||||
|
#include <math.h>
|
||||||
|
#include <serial/serial.h>
|
||||||
|
#include "geometry_msgs/PointStamped.h"
|
||||||
|
#include "tf2_ros/transform_listener.h"
|
||||||
|
#include "tf2_ros/buffer.h"
|
||||||
|
#include "std_msgs/Float64.h"
|
||||||
|
|
||||||
|
#define pi 3.141592653
|
||||||
|
|
||||||
|
//前后左右停:a,b,c,d,e
|
||||||
|
|
||||||
|
char cmd='a';
|
||||||
|
|
||||||
|
int main(int argc,char* argv[])
|
||||||
|
{
|
||||||
|
setlocale(LC_ALL,"");
|
||||||
|
ros::init(argc,argv,"gou2_test");
|
||||||
|
ros::NodeHandle nh;
|
||||||
|
ros::Publisher joint_pub=nh.advertise<sensor_msgs::JointState>("joint_states",1000);
|
||||||
|
|
||||||
|
ros::Publisher lf_jian_pu=nh.advertise<std_msgs::Float64>("/gou2/lf_jian_con/command",1000);
|
||||||
|
ros::Publisher lf_zhou_pu=nh.advertise<std_msgs::Float64>("/gou2/lf_zhou_con/command",1000);
|
||||||
|
ros::Publisher lf_xi_pu=nh.advertise<std_msgs::Float64>("/gou2/lf_xi_con/command",1000);
|
||||||
|
ros::Publisher lb_jian_pu=nh.advertise<std_msgs::Float64>("/gou2/lb_jian_con/command",1000);
|
||||||
|
ros::Publisher lb_zhou_pu=nh.advertise<std_msgs::Float64>("/gou2/lb_zhou_con/command",1000);
|
||||||
|
ros::Publisher lb_xi_pu=nh.advertise<std_msgs::Float64>("/gou2/lb_xi_con/command",1000);
|
||||||
|
ros::Publisher rf_jian_pu=nh.advertise<std_msgs::Float64>("/gou2/rf_jian_con/command",1000);
|
||||||
|
ros::Publisher rf_zhou_pu=nh.advertise<std_msgs::Float64>("/gou2/rf_zhou_con/command",1000);
|
||||||
|
ros::Publisher rf_xi_pu=nh.advertise<std_msgs::Float64>("/gou2/rf_xi_con/command",1000);
|
||||||
|
ros::Publisher rb_jian_pu=nh.advertise<std_msgs::Float64>("/gou2/rb_jian_con/command",1000);
|
||||||
|
ros::Publisher rb_zhou_pu=nh.advertise<std_msgs::Float64>("/gou2/rb_zhou_con/command",1000);
|
||||||
|
ros::Publisher rb_xi_pu=nh.advertise<std_msgs::Float64>("/gou2/rb_xi_con/command",1000);
|
||||||
|
|
||||||
|
//定义tf坐标广播器
|
||||||
|
tf2_ros::StaticTransformBroadcaster broadcaster;
|
||||||
|
geometry_msgs::TransformStamped ts;
|
||||||
|
|
||||||
|
ts.header.seq=100;
|
||||||
|
ts.header.stamp=ros::Time::now();
|
||||||
|
ts.header.frame_id="world";
|
||||||
|
ts.child_frame_id="base_link";
|
||||||
|
ts.transform.translation.x=0.0;
|
||||||
|
ts.transform.translation.y=0.0;
|
||||||
|
ts.transform.translation.z=0.0805;
|
||||||
|
|
||||||
|
tf2::Quaternion qtn;
|
||||||
|
qtn.setRPY(0,0,0);
|
||||||
|
ts.transform.rotation.x=qtn.getX();
|
||||||
|
ts.transform.rotation.y=qtn.getY();
|
||||||
|
ts.transform.rotation.z=qtn.getZ();
|
||||||
|
ts.transform.rotation.w=qtn.getW();
|
||||||
|
broadcaster.sendTransform(ts);
|
||||||
|
/*
|
||||||
|
//创建一个serial对象
|
||||||
|
serial::Serial sp;
|
||||||
|
serial::Timeout to = serial::Timeout::simpleTimeout(10);
|
||||||
|
//设置要打开的串口名称
|
||||||
|
sp.setPort("/dev/ttyUSB0");
|
||||||
|
//设置串口通信的波特率
|
||||||
|
sp.setBaudrate(9600);
|
||||||
|
//串口设置timeout
|
||||||
|
sp.setTimeout(to);
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
//打开串口
|
||||||
|
sp.open();
|
||||||
|
}
|
||||||
|
catch(serial::IOException& e)
|
||||||
|
{
|
||||||
|
ROS_ERROR_STREAM("Unable to open port.");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
//判断串口是否打开成功
|
||||||
|
if(sp.isOpen())
|
||||||
|
{
|
||||||
|
ROS_INFO_STREAM("/dev/ttyUSB0 is opened.");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
*/
|
||||||
|
sensor_msgs::JointState joint_state;
|
||||||
|
|
||||||
|
std::string robot_desc_string;
|
||||||
|
nh.param("robot1_description", robot_desc_string, std::string());
|
||||||
|
KDL::Tree my_tree;
|
||||||
|
if(!kdl_parser::treeFromString(robot_desc_string, my_tree))
|
||||||
|
//if(!kdl_parser::treeFromFile("/home/zhitong/catkin_ws_serial/src/Gou2/urdf/Gou2.urdf", my_tree))
|
||||||
|
{
|
||||||
|
ROS_ERROR("Failed to construct kdl tree");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
ROS_INFO("成功生成kdl树!");
|
||||||
|
}
|
||||||
|
std::vector<std::string> joint_name = {
|
||||||
|
"LF_JIAN_JOINT", "LF_ZHOU_JOINT", "LF_XI_JOINT", "LF_R_JOINT","LF_P_JOINT", "LF_Y_JOINT",
|
||||||
|
"LB_JIAN_JOINT", "LB_ZHOU_JOINT", "LB_XI_JOINT", "LB_R_JOINT","LB_P_JOINT", "LB_Y_JOINT",
|
||||||
|
"RF_JIAN_JOINT", "RF_ZHOU_JOINT", "RF_XI_JOINT", "RF_R_JOINT","RF_P_JOINT", "RF_Y_JOINT",
|
||||||
|
"RB_JIAN_JOINT", "RB_ZHOU_JOINT", "RB_XI_JOINT", "RB_R_JOINT","RB_P_JOINT", "RB_Y_JOINT"
|
||||||
|
};
|
||||||
|
std::vector<double> joint_pos = {
|
||||||
|
0,0,0,0,0,0,0,
|
||||||
|
0,0,0,0,0,0,0,
|
||||||
|
0,0,0,0,0,0,0,
|
||||||
|
0,0,0,0,0,0,0
|
||||||
|
};
|
||||||
|
|
||||||
|
std::string urdf_param = "/robot1_description";
|
||||||
|
double timeout = 0.005;
|
||||||
|
double eps = 1e-5;
|
||||||
|
std::string chain_start = "base_link";
|
||||||
|
std::string chain_end = "LF_Y_LINK";
|
||||||
|
TRAC_IK::TRAC_IK tracik_solver(chain_start, chain_end, urdf_param, timeout, eps);
|
||||||
|
KDL::Chain chain;
|
||||||
|
KDL::JntArray ll, ul; //关节下限, 关节上限
|
||||||
|
bool valid = tracik_solver.getKDLChain(chain);
|
||||||
|
if(!valid)
|
||||||
|
{
|
||||||
|
ROS_ERROR("There was no valid KDL chain found");
|
||||||
|
}
|
||||||
|
valid = tracik_solver.getKDLLimits(ll, ul);
|
||||||
|
if(!valid)
|
||||||
|
{
|
||||||
|
ROS_ERROR("There was no valid KDL joint limits found");
|
||||||
|
}
|
||||||
|
KDL::ChainFkSolverPos_recursive fk_solver(chain);
|
||||||
|
ROS_INFO("关节数量: %d", chain.getNrOfJoints());
|
||||||
|
|
||||||
|
KDL::JntArray q(6); // 初始关节位置
|
||||||
|
q(0)=0;
|
||||||
|
q(1)=-pi/6;
|
||||||
|
q(2)=pi/6;
|
||||||
|
q(3)=0;
|
||||||
|
q(4)=0;
|
||||||
|
q(5)=0;
|
||||||
|
//定义末端4x4齐次变换矩阵(各个关节位于初始位置时，末端齐次矩阵)
|
||||||
|
KDL::Frame end_effector_pose1_l;
|
||||||
|
KDL::Frame end_effector_pose1_r;
|
||||||
|
//定义末端4x4齐次变换矩阵(各个关节的实时末端齐次矩阵)
|
||||||
|
KDL::Frame end_effector_pose_l;//left now
|
||||||
|
KDL::Frame end_effector_pose_r;//left now
|
||||||
|
|
||||||
|
//定义逆运动学解算结果存储数组
|
||||||
|
KDL::JntArray result_l;//left
|
||||||
|
KDL::JntArray result_r;//right
|
||||||
|
//上一帧的关节变量
|
||||||
|
KDL::JntArray resu_last_l;//left last
|
||||||
|
KDL::JntArray resu_last_r;//right last
|
||||||
|
ros::Rate r(40);
|
||||||
|
|
||||||
|
auto print_frame_lambda = [](KDL::Frame f)
|
||||||
|
{
|
||||||
|
double x, y, z, roll, pitch, yaw;
|
||||||
|
x = f.p.x();
|
||||||
|
y = f.p.y();
|
||||||
|
z = f.p.z();
|
||||||
|
f.M.GetRPY(roll, pitch, yaw);
|
||||||
|
std::cout << "x:" << x << " y:" << y << " z:" << z << " roll:" << roll << " pitch:" << pitch << " yaw:" << yaw << std::endl;
|
||||||
|
};
|
||||||
|
|
||||||
|
//正运动学求初始状态齐次变换矩阵
|
||||||
|
fk_solver.JntToCart(q,end_effector_pose1_l);
|
||||||
|
fk_solver.JntToCart(q,end_effector_pose1_r);
|
||||||
|
print_frame_lambda(end_effector_pose1_l);
|
||||||
|
|
||||||
|
ROS_INFO("更新关节状态");
|
||||||
|
joint_state.header.stamp = ros::Time::now();
|
||||||
|
//ROS_INFO("%d",joint_state.header.stamp);
|
||||||
|
joint_state.name.resize(24);
|
||||||
|
joint_state.position.resize(24);
|
||||||
|
|
||||||
|
for(size_t i=0;i<=23;i++)
|
||||||
|
{
|
||||||
|
joint_state.name[i] = joint_name[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
for(int i=0;i<=23;i++)
|
||||||
|
{
|
||||||
|
joint_state.position[i]=q(i%6);
|
||||||
|
}
|
||||||
|
joint_pub.publish(joint_state);
|
||||||
|
|
||||||
|
fk_solver.JntToCart(q, end_effector_pose_l);
|
||||||
|
fk_solver.JntToCart(q, end_effector_pose_r);
|
||||||
|
|
||||||
|
resu_last_l=q;
|
||||||
|
resu_last_r=q;
|
||||||
|
|
||||||
|
double r_b=0.0,p_b=0.0,y_b=0.0;
|
||||||
|
|
||||||
|
geometry_msgs::PointStamped point_base;
|
||||||
|
point_base.header.frame_id="base_link";
|
||||||
|
point_base.header.stamp=ros::Time::now();
|
||||||
|
point_base.point.x=0;
|
||||||
|
point_base.point.y=0;
|
||||||
|
point_base.point.z=0;
|
||||||
|
|
||||||
|
tf2_ros::Buffer BUffer;
|
||||||
|
tf2_ros::TransformListener listener(BUffer);
|
||||||
|
|
||||||
|
while(ros::ok())
|
||||||
|
{
|
||||||
|
cmd='a';
|
||||||
|
|
||||||
|
/*
|
||||||
|
size_t n = sp.available();
|
||||||
|
if(n!=0)
|
||||||
|
{
|
||||||
|
uint8_t buffer[1024];
|
||||||
|
n = sp.read(buffer, n);
|
||||||
|
cmd=buffer[0];
|
||||||
|
}
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
||||||
|
if(cmd=='a')
|
||||||
|
{
|
||||||
|
//x=0.02*(t-sint);
|
||||||
|
//y=0.02*(1-cost);
|
||||||
|
for(int i=1;i<=20;i++)
|
||||||
|
{
|
||||||
|
double t=2*pi*i/20;
|
||||||
|
double x=0.005*(t-sin(t));
|
||||||
|
double z=0.005*(1-cos(t));
|
||||||
|
end_effector_pose_l.p.data[0]=end_effector_pose1_l.p.data[0]+x;
|
||||||
|
end_effector_pose_l.p.data[2]=end_effector_pose1_l.p.data[2]+z;
|
||||||
|
int rc_l = tracik_solver.CartToJnt(resu_last_l, end_effector_pose_l, result_l);
|
||||||
|
|
||||||
|
end_effector_pose_r.p.data[0]=end_effector_pose1_r.p.data[0]+0.005*2*pi-x;
|
||||||
|
end_effector_pose_r.p.data[2]=end_effector_pose1_r.p.data[2];
|
||||||
|
int rc_r = tracik_solver.CartToJnt(resu_last_r, end_effector_pose_r, result_r);
|
||||||
|
|
||||||
|
//ROS_INFO("%f,%f,%f,%f,%f,%f",result_l(0),result_l(1),result_l(2),result_l(3),result_l(4),result_l(5));
|
||||||
|
|
||||||
|
joint_state.header.stamp = ros::Time::now();
|
||||||
|
|
||||||
|
for(int j=0;j<=23;j++)
|
||||||
|
{
|
||||||
|
joint_state.position[j]=((j/6==0)||(j/6==3))?result_l(j%6):result_r(j%6);
|
||||||
|
}
|
||||||
|
joint_pub.publish(joint_state);
|
||||||
|
|
||||||
|
|
||||||
|
std_msgs::Float64 joint_po;
|
||||||
|
|
||||||
|
joint_po.data=joint_state.position[0];
|
||||||
|
lf_jian_pu.publish(joint_po);
|
||||||
|
joint_po.data=joint_state.position[1];
|
||||||
|
lf_zhou_pu.publish(joint_po);
|
||||||
|
joint_po.data=joint_state.position[2];
|
||||||
|
lf_xi_pu.publish(joint_po);
|
||||||
|
|
||||||
|
joint_po.data=joint_state.position[6];
|
||||||
|
lb_jian_pu.publish(joint_po);
|
||||||
|
joint_po.data=joint_state.position[7];
|
||||||
|
lb_zhou_pu.publish(joint_po);
|
||||||
|
joint_po.data=joint_state.position[8];
|
||||||
|
lb_xi_pu.publish(joint_po);
|
||||||
|
|
||||||
|
joint_po.data=joint_state.position[12];
|
||||||
|
rf_jian_pu.publish(joint_po);
|
||||||
|
joint_po.data=joint_state.position[13];
|
||||||
|
rf_zhou_pu.publish(joint_po);
|
||||||
|
joint_po.data=joint_state.position[14];
|
||||||
|
rf_xi_pu.publish(joint_po);
|
||||||
|
|
||||||
|
joint_po.data=joint_state.position[18];
|
||||||
|
rb_jian_pu.publish(joint_po);
|
||||||
|
joint_po.data=joint_state.position[19];
|
||||||
|
rb_zhou_pu.publish(joint_po);
|
||||||
|
joint_po.data=joint_state.position[20];
|
||||||
|
rb_xi_pu.publish(joint_po);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
ts.header.stamp=ros::Time::now();
|
||||||
|
point_base.header.stamp=ros::Time::now();
|
||||||
|
point_base.point.x=0.00025*2*pi;
|
||||||
|
point_base.point.y=0;
|
||||||
|
point_base.point.z=0;
|
||||||
|
|
||||||
|
geometry_msgs::PointStamped point_world;
|
||||||
|
point_world=BUffer.transform(point_base,"world");
|
||||||
|
|
||||||
|
ts.transform.translation.x=point_world.point.x;
|
||||||
|
ts.transform.translation.y=point_world.point.y;
|
||||||
|
ts.transform.translation.z=point_world.point.z;
|
||||||
|
|
||||||
|
point_base.point.x=0;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
ts.transform.translation.x+=0.00025*2*pi;
|
||||||
|
ts.transform.translation.y=0.0;
|
||||||
|
ts.transform.translation.z=0.0805;
|
||||||
|
|
||||||
|
|
||||||
|
qtn.setRPY(r_b,p_b,y_b);
|
||||||
|
ts.transform.rotation.x=qtn.getX();
|
||||||
|
ts.transform.rotation.y=qtn.getY();
|
||||||
|
ts.transform.rotation.z=qtn.getZ();
|
||||||
|
ts.transform.rotation.w=qtn.getW();
|
||||||
|
broadcaster.sendTransform(ts);
|
||||||
|
|
||||||
|
resu_last_l=result_l;
|
||||||
|
resu_last_r=result_r;
|
||||||
|
r.sleep();
|
||||||
|
}
|
||||||
|
|
||||||
|
for(int i=1;i<=20;i++)
|
||||||
|
{
|
||||||
|
double t=2*pi*i/20;
|
||||||
|
double x=0.005*(t-sin(t));
|
||||||
|
double z=0.005*(1-cos(t));
|
||||||
|
end_effector_pose_l.p.data[0]=end_effector_pose1_l.p.data[0]+0.005*2*pi-x;
|
||||||
|
end_effector_pose_l.p.data[2]=end_effector_pose1_l.p.data[2];
|
||||||
|
int rc_l = tracik_solver.CartToJnt(resu_last_l, end_effector_pose_l, result_l);
|
||||||
|
|
||||||
|
end_effector_pose_r.p.data[0]=end_effector_pose1_r.p.data[0]+x;
|
||||||
|
end_effector_pose_r.p.data[2]=end_effector_pose1_r.p.data[2]+z;
|
||||||
|
int rc_r = tracik_solver.CartToJnt(resu_last_r, end_effector_pose_r, result_r);
|
||||||
|
|
||||||
|
joint_state.header.stamp = ros::Time::now();
|
||||||
|
|
||||||
|
for(int j=0;j<=23;j++)
|
||||||
|
{
|
||||||
|
joint_state.position[j]=((j/6==0)||(j/6==3))?result_l(j%6):result_r(j%6);
|
||||||
|
}
|
||||||
|
|
||||||
|
joint_pub.publish(joint_state);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
std_msgs::Float64 joint_po;
|
||||||
|
ROS_INFO("hello");
|
||||||
|
joint_po.data=joint_state.position[0];
|
||||||
|
lf_jian_pu.publish(joint_po);
|
||||||
|
joint_po.data=joint_state.position[1];
|
||||||
|
lf_zhou_pu.publish(joint_po);
|
||||||
|
joint_po.data=joint_state.position[2];
|
||||||
|
lf_xi_pu.publish(joint_po);
|
||||||
|
|
||||||
|
joint_po.data=joint_state.position[6];
|
||||||
|
lb_jian_pu.publish(joint_po);
|
||||||
|
joint_po.data=joint_state.position[7];
|
||||||
|
lb_zhou_pu.publish(joint_po);
|
||||||
|
joint_po.data=joint_state.position[8];
|
||||||
|
lb_xi_pu.publish(joint_po);
|
||||||
|
|
||||||
|
joint_po.data=joint_state.position[12];
|
||||||
|
rf_jian_pu.publish(joint_po);
|
||||||
|
joint_po.data=joint_state.position[13];
|
||||||
|
rf_zhou_pu.publish(joint_po);
|
||||||
|
joint_po.data=joint_state.position[14];
|
||||||
|
rf_xi_pu.publish(joint_po);
|
||||||
|
|
||||||
|
joint_po.data=joint_state.position[18];
|
||||||
|
rb_jian_pu.publish(joint_po);
|
||||||
|
joint_po.data=joint_state.position[19];
|
||||||
|
rb_zhou_pu.publish(joint_po);
|
||||||
|
joint_po.data=joint_state.position[20];
|
||||||
|
rb_xi_pu.publish(joint_po);
|
||||||
|
|
||||||
|
|
||||||
|
ts.header.stamp=ros::Time::now();
|
||||||
|
point_base.header.stamp=ros::Time::now();
|
||||||
|
point_base.point.x=0.00025*2*pi;
|
||||||
|
point_base.point.y=0;
|
||||||
|
point_base.point.z=0;
|
||||||
|
|
||||||
|
geometry_msgs::PointStamped point_world;
|
||||||
|
point_world=BUffer.transform(point_base,"world");
|
||||||
|
|
||||||
|
ts.transform.translation.x=point_world.point.x;
|
||||||
|
ts.transform.translation.y=point_world.point.y;
|
||||||
|
ts.transform.translation.z=point_world.point.z;
|
||||||
|
|
||||||
|
point_base.point.x=0.0;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
ts.transform.translation.x+=0.00025*2*pi;
|
||||||
|
ts.transform.translation.y=0.0;
|
||||||
|
ts.transform.translation.z=0.0805;
|
||||||
|
|
||||||
|
qtn.setRPY(r_b,p_b,y_b);
|
||||||
|
ts.transform.rotation.x=qtn.getX();
|
||||||
|
ts.transform.rotation.y=qtn.getY();
|
||||||
|
ts.transform.rotation.z=qtn.getZ();
|
||||||
|
ts.transform.rotation.w=qtn.getW();
|
||||||
|
broadcaster.sendTransform(ts);
|
||||||
|
|
||||||
|
resu_last_l=result_l;
|
||||||
|
resu_last_r=result_r;
|
||||||
|
r.sleep();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if(cmd=='b')
|
||||||
|
{
|
||||||
|
//x=0.02*(t-sint);
|
||||||
|
//y=0.02*(1-cost);
|
||||||
|
for(int i=1;i<=20;i++)
|
||||||
|
{
|
||||||
|
double t=2*pi*i/20;
|
||||||
|
double x=0.005*(t-sin(t));
|
||||||
|
double z=0.005*(1-cos(t));
|
||||||
|
end_effector_pose_l.p.data[0]=end_effector_pose1_l.p.data[0]+0.005*2*pi-x;
|
||||||
|
end_effector_pose_l.p.data[2]=end_effector_pose1_l.p.data[2]+z;
|
||||||
|
int rc_l = tracik_solver.CartToJnt(resu_last_l, end_effector_pose_l, result_l);
|
||||||
|
|
||||||
|
end_effector_pose_r.p.data[0]=end_effector_pose1_r.p.data[0]+x;
|
||||||
|
end_effector_pose_r.p.data[2]=end_effector_pose1_r.p.data[2];
|
||||||
|
int rc_r = tracik_solver.CartToJnt(resu_last_r, end_effector_pose_r, result_r);
|
||||||
|
|
||||||
|
joint_state.header.stamp = ros::Time::now();
|
||||||
|
|
||||||
|
for(int j=0;j<=23;j++)
|
||||||
|
{
|
||||||
|
joint_state.position[j]=((j/6==0)||(j/6==3))?result_l(j%6):result_r(j%6);
|
||||||
|
}
|
||||||
|
joint_pub.publish(joint_state);
|
||||||
|
|
||||||
|
ts.header.stamp=ros::Time::now();
|
||||||
|
point_base.header.stamp=ros::Time::now();
|
||||||
|
point_base.point.x=-0.00025*2*pi;
|
||||||
|
point_base.point.y=0;
|
||||||
|
point_base.point.z=0;
|
||||||
|
|
||||||
|
geometry_msgs::PointStamped point_world;
|
||||||
|
point_world=BUffer.transform(point_base,"world");
|
||||||
|
|
||||||
|
ts.transform.translation.x=point_world.point.x;
|
||||||
|
ts.transform.translation.y=point_world.point.y;
|
||||||
|
ts.transform.translation.z=point_world.point.z;
|
||||||
|
|
||||||
|
point_base.point.x=0;
|
||||||
|
|
||||||
|
/*
|
||||||
|
ts.transform.translation.x-=0.00025*2*pi;
|
||||||
|
ts.transform.translation.y=0.0;
|
||||||
|
ts.transform.translation.z=0.0805;
|
||||||
|
*/
|
||||||
|
qtn.setRPY(r_b,p_b,y_b);
|
||||||
|
ts.transform.rotation.x=qtn.getX();
|
||||||
|
ts.transform.rotation.y=qtn.getY();
|
||||||
|
ts.transform.rotation.z=qtn.getZ();
|
||||||
|
ts.transform.rotation.w=qtn.getW();
|
||||||
|
broadcaster.sendTransform(ts);
|
||||||
|
|
||||||
|
resu_last_l=result_l;
|
||||||
|
resu_last_r=result_r;
|
||||||
|
r.sleep();
|
||||||
|
}
|
||||||
|
|
||||||
|
for(int i=1;i<=20;i++)
|
||||||
|
{
|
||||||
|
double t=2*pi*i/20;
|
||||||
|
double x=0.005*(t-sin(t));
|
||||||
|
double z=0.005*(1-cos(t));
|
||||||
|
end_effector_pose_l.p.data[0]=end_effector_pose1_l.p.data[0]+x;
|
||||||
|
end_effector_pose_l.p.data[2]=end_effector_pose1_l.p.data[2];
|
||||||
|
int rc_l = tracik_solver.CartToJnt(resu_last_l, end_effector_pose_l, result_l);
|
||||||
|
|
||||||
|
end_effector_pose_r.p.data[0]=end_effector_pose1_r.p.data[0]+0.005*2*pi-x;
|
||||||
|
end_effector_pose_r.p.data[2]=end_effector_pose1_r.p.data[2]+z;
|
||||||
|
int rc_r = tracik_solver.CartToJnt(resu_last_r, end_effector_pose_r, result_r);
|
||||||
|
|
||||||
|
joint_state.header.stamp = ros::Time::now();
|
||||||
|
|
||||||
|
for(int j=0;j<=23;j++)
|
||||||
|
{
|
||||||
|
joint_state.position[j]=((j/6==0)||(j/6==3))?result_l(j%6):result_r(j%6);
|
||||||
|
}
|
||||||
|
|
||||||
|
joint_pub.publish(joint_state);
|
||||||
|
|
||||||
|
ts.header.stamp=ros::Time::now();
|
||||||
|
point_base.header.stamp=ros::Time::now();
|
||||||
|
point_base.point.x=-0.00025*2*pi;
|
||||||
|
point_base.point.y=0;
|
||||||
|
point_base.point.z=0;
|
||||||
|
|
||||||
|
geometry_msgs::PointStamped point_world;
|
||||||
|
point_world=BUffer.transform(point_base,"world");
|
||||||
|
|
||||||
|
ts.transform.translation.x=point_world.point.x;
|
||||||
|
ts.transform.translation.y=point_world.point.y;
|
||||||
|
ts.transform.translation.z=point_world.point.z;
|
||||||
|
|
||||||
|
point_base.point.x=0;
|
||||||
|
/*
|
||||||
|
ts.transform.translation.x-=0.00025*2*pi;
|
||||||
|
ts.transform.translation.y=0.0;
|
||||||
|
ts.transform.translation.z=0.0805;
|
||||||
|
*/
|
||||||
|
qtn.setRPY(r_b,p_b,y_b);
|
||||||
|
ts.transform.rotation.x=qtn.getX();
|
||||||
|
ts.transform.rotation.y=qtn.getY();
|
||||||
|
ts.transform.rotation.z=qtn.getZ();
|
||||||
|
ts.transform.rotation.w=qtn.getW();
|
||||||
|
broadcaster.sendTransform(ts);
|
||||||
|
|
||||||
|
resu_last_l=result_l;
|
||||||
|
resu_last_r=result_r;
|
||||||
|
r.sleep();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if(cmd=='c')
|
||||||
|
{
|
||||||
|
//x=0.02*(t-sint);
|
||||||
|
//y=0.02*(1-cost);
|
||||||
|
for(int i=1;i<=20;i++)
|
||||||
|
{
|
||||||
|
double t=2*pi*i/20;
|
||||||
|
double x=0.005*(t-sin(t));
|
||||||
|
double z=0.005*(1-cos(t));
|
||||||
|
end_effector_pose_l.p.data[0]=end_effector_pose1_l.p.data[0]+0.005*2*pi-x;
|
||||||
|
end_effector_pose_l.p.data[2]=end_effector_pose1_l.p.data[2]+z;
|
||||||
|
int rc_l = tracik_solver.CartToJnt(resu_last_l, end_effector_pose_l, result_l);
|
||||||
|
|
||||||
|
end_effector_pose_r.p.data[0]=end_effector_pose1_r.p.data[0]+0.005*2*pi-x;
|
||||||
|
end_effector_pose_r.p.data[2]=end_effector_pose1_r.p.data[2];
|
||||||
|
int rc_r = tracik_solver.CartToJnt(resu_last_r, end_effector_pose_r, result_r);
|
||||||
|
|
||||||
|
joint_state.header.stamp = ros::Time::now();
|
||||||
|
|
||||||
|
for(int j=0;j<=23;j++)
|
||||||
|
{
|
||||||
|
joint_state.position[j]=((j/6==0)||(j/6==3))?result_l(j%6):result_r(j%6);
|
||||||
|
}
|
||||||
|
joint_pub.publish(joint_state);
|
||||||
|
|
||||||
|
ts.header.stamp=ros::Time::now();
|
||||||
|
y_b+=pi/(20*20*2);
|
||||||
|
qtn.setRPY(r_b,p_b,y_b);
|
||||||
|
ts.transform.rotation.x=qtn.getX();
|
||||||
|
ts.transform.rotation.y=qtn.getY();
|
||||||
|
ts.transform.rotation.z=qtn.getZ();
|
||||||
|
ts.transform.rotation.w=qtn.getW();
|
||||||
|
broadcaster.sendTransform(ts);
|
||||||
|
|
||||||
|
resu_last_l=result_l;
|
||||||
|
resu_last_r=result_r;
|
||||||
|
r.sleep();
|
||||||
|
}
|
||||||
|
|
||||||
|
for(int i=1;i<=20;i++)
|
||||||
|
{
|
||||||
|
double t=2*pi*i/20;
|
||||||
|
double x=0.005*(t-sin(t));
|
||||||
|
double z=0.005*(1-cos(t));
|
||||||
|
end_effector_pose_l.p.data[0]=end_effector_pose1_l.p.data[0]+x;
|
||||||
|
end_effector_pose_l.p.data[2]=end_effector_pose1_l.p.data[2];
|
||||||
|
int rc_l = tracik_solver.CartToJnt(resu_last_l, end_effector_pose_l, result_l);
|
||||||
|
|
||||||
|
end_effector_pose_r.p.data[0]=end_effector_pose1_r.p.data[0]+x;
|
||||||
|
end_effector_pose_r.p.data[2]=end_effector_pose1_r.p.data[2]+z;
|
||||||
|
int rc_r = tracik_solver.CartToJnt(resu_last_r, end_effector_pose_r, result_r);
|
||||||
|
|
||||||
|
joint_state.header.stamp = ros::Time::now();
|
||||||
|
|
||||||
|
for(int j=0;j<=23;j++)
|
||||||
|
{
|
||||||
|
joint_state.position[j]=((j/6==0)||(j/6==3))?result_l(j%6):result_r(j%6);
|
||||||
|
}
|
||||||
|
|
||||||
|
joint_pub.publish(joint_state);
|
||||||
|
|
||||||
|
ts.header.stamp=ros::Time::now();
|
||||||
|
y_b+=pi/(20*20*2);
|
||||||
|
qtn.setRPY(r_b,p_b,y_b);
|
||||||
|
ts.transform.rotation.x=qtn.getX();
|
||||||
|
ts.transform.rotation.y=qtn.getY();
|
||||||
|
ts.transform.rotation.z=qtn.getZ();
|
||||||
|
ts.transform.rotation.w=qtn.getW();
|
||||||
|
broadcaster.sendTransform(ts);
|
||||||
|
|
||||||
|
resu_last_l=result_l;
|
||||||
|
resu_last_r=result_r;
|
||||||
|
r.sleep();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if(cmd=='d')
|
||||||
|
{
|
||||||
|
//x=0.02*(t-sint);
|
||||||
|
//y=0.02*(1-cost);
|
||||||
|
for(int i=1;i<=20;i++)
|
||||||
|
{
|
||||||
|
double t=2*pi*i/20;
|
||||||
|
double x=0.005*(t-sin(t));
|
||||||
|
double z=0.005*(1-cos(t));
|
||||||
|
|
||||||
|
end_effector_pose_l.p.data[0]=end_effector_pose1_l.p.data[0]+x;
|
||||||
|
end_effector_pose_l.p.data[2]=end_effector_pose1_l.p.data[2]+z;
|
||||||
|
int rc_l = tracik_solver.CartToJnt(resu_last_l, end_effector_pose_l, result_l);
|
||||||
|
|
||||||
|
end_effector_pose_r.p.data[0]=end_effector_pose1_r.p.data[0]+x;
|
||||||
|
end_effector_pose_r.p.data[2]=end_effector_pose1_r.p.data[2];
|
||||||
|
int rc_r = tracik_solver.CartToJnt(resu_last_r, end_effector_pose_r, result_r);
|
||||||
|
|
||||||
|
joint_state.header.stamp = ros::Time::now();
|
||||||
|
|
||||||
|
for(int j=0;j<=23;j++)
|
||||||
|
{
|
||||||
|
joint_state.position[j]=((j/6==0)||(j/6==3))?result_l(j%6):result_r(j%6);
|
||||||
|
}
|
||||||
|
joint_pub.publish(joint_state);
|
||||||
|
|
||||||
|
ts.header.stamp=ros::Time::now();
|
||||||
|
y_b-=pi/(20*20*2);
|
||||||
|
qtn.setRPY(r_b,p_b,y_b);
|
||||||
|
ts.transform.rotation.x=qtn.getX();
|
||||||
|
ts.transform.rotation.y=qtn.getY();
|
||||||
|
ts.transform.rotation.z=qtn.getZ();
|
||||||
|
ts.transform.rotation.w=qtn.getW();
|
||||||
|
broadcaster.sendTransform(ts);
|
||||||
|
|
||||||
|
resu_last_l=result_l;
|
||||||
|
resu_last_r=result_r;
|
||||||
|
r.sleep();
|
||||||
|
}
|
||||||
|
|
||||||
|
for(int i=1;i<=20;i++)
|
||||||
|
{
|
||||||
|
double t=2*pi*i/20;
|
||||||
|
double x=0.005*(t-sin(t));
|
||||||
|
double z=0.005*(1-cos(t));
|
||||||
|
end_effector_pose_l.p.data[0]=end_effector_pose1_l.p.data[0]+(0.005*2*pi-x);
|
||||||
|
end_effector_pose_l.p.data[2]=end_effector_pose1_l.p.data[2];
|
||||||
|
int rc_l = tracik_solver.CartToJnt(resu_last_l, end_effector_pose_l, result_l);
|
||||||
|
|
||||||
|
end_effector_pose_r.p.data[0]=end_effector_pose1_r.p.data[0]+0.005*2*pi-x;
|
||||||
|
end_effector_pose_r.p.data[2]=end_effector_pose1_r.p.data[2]+z;
|
||||||
|
int rc_r = tracik_solver.CartToJnt(resu_last_r, end_effector_pose_r, result_r);
|
||||||
|
|
||||||
|
joint_state.header.stamp = ros::Time::now();
|
||||||
|
|
||||||
|
for(int j=0;j<=23;j++)
|
||||||
|
{
|
||||||
|
joint_state.position[j]=((j/6==0)||(j/6==3))?result_l(j%6):result_r(j%6);
|
||||||
|
}
|
||||||
|
|
||||||
|
joint_pub.publish(joint_state);
|
||||||
|
|
||||||
|
ts.header.stamp=ros::Time::now();
|
||||||
|
y_b-=pi/(20*20*2);
|
||||||
|
qtn.setRPY(r_b,p_b,y_b);
|
||||||
|
ts.transform.rotation.x=qtn.getX();
|
||||||
|
ts.transform.rotation.y=qtn.getY();
|
||||||
|
ts.transform.rotation.z=qtn.getZ();
|
||||||
|
ts.transform.rotation.w=qtn.getW();
|
||||||
|
broadcaster.sendTransform(ts);
|
||||||
|
|
||||||
|
resu_last_l=result_l;
|
||||||
|
resu_last_r=result_r;
|
||||||
|
r.sleep();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if(cmd=='e')
|
||||||
|
r.sleep();
|
||||||
|
else;
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -0,0 +1,76 @@
|
||||||
|
lf_jian_con
|
||||||
|
lb_jian_con
|
||||||
|
rf_jian_con
|
||||||
|
rb_jian_con
|
||||||
|
|
||||||
|
|
||||||
|
lf_jian_con:
|
||||||
|
type: position_controllers/JointPositionController
|
||||||
|
joint: LF_JIAN_JOINT
|
||||||
|
#pid: {p: 1.0, i: 0.01, d: 0.0}
|
||||||
|
lb_jian_con:
|
||||||
|
type: position_controllers/JointPositionController
|
||||||
|
joint: LB_JIAN_JOINT
|
||||||
|
#pid: {p: 1.0, i: 0.01, d: 0.0}
|
||||||
|
rf_jian_con:
|
||||||
|
type: position_controllers/JointPositionController
|
||||||
|
joint: RF_JIAN_JOINT
|
||||||
|
#pid: {p: 1.0, i: 0.01, d: 0.0}
|
||||||
|
rb_jian_con:
|
||||||
|
type: position_controllers/JointPositionController
|
||||||
|
joint: RB_JIAN_JOINT
|
||||||
|
#pid: {p: 1.0, i: 0.01, d: 0.0}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
LF_JIAN_JOINT: {p: 1.0, i: 0.0, d: 0.0}
|
||||||
|
LB_JIAN_JOINT: {p: 1.0, i: 0.0, d: 0.0}
|
||||||
|
RF_JIAN_JOINT: {p: 1.0, i: 0.0, d: 0.0}
|
||||||
|
RB_JIAN_JOINT: {p: 1.0, i: 0.0, d: 0.0}
|
||||||
|
|
||||||
|
|
||||||
|
<transmission name="lf_jian_trans">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="LF_JIAN_JOINT">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="lf_jian_motor">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction> 1 </mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
|
||||||
|
<transmission name="lb_jian_trans">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="LB_JIAN_JOINT">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="lb_jian_motor">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction> 1 </mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
|
||||||
|
<transmission name="rf_jian_trans">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="RF_JIAN_JOINT">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="rf_jian_motor">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction> 1 </mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
|
||||||
|
<transmission name="rb_jian_trans">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="RB_JIAN_JOINT">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="rb_jian_motor">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction> 1 </mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,26 @@
|
||||||
|
Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
|
||||||
|
base_link,0.00239086434033531,0.000199255741624531,-0.00013079370812909,0,0,0,0.941484635627895,0.000209928253043728,1.16253598509181E-06,-7.59914437724361E-08,0.00122280602530151,-2.21405921740874E-11,0.00140751862624959,0,0,0,0,0,0,package://gou2/meshes/base_link.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://gou2/meshes/base_link.STL,,JIJIA-1;mpu6050-1,坐标系1,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
|
||||||
|
LF_JIAN_LINK,-0.0539657436881796,0.0119844204986863,0.0593375454484812,0,0,0,1.39729963569281,0.00458289280415563,2.64159261572492E-06,-1.42747619958464E-06,0.00534215746371847,1.28385425078759E-05,0.00190255271211888,0,0,0,0,0,0,package://gou2/meshes/LF_JIAN_LINK.STL,0.529411764705882,0.549019607843137,0.549019607843137,1,0,0,0,0,0,0,package://gou2/meshes/LF_JIAN_LINK.STL,,Gou-2-xuniguanjie-1,坐标系2,基准轴2,LF_JIAN_JOINT,revolute,0.062375,0.0541,0,-1.5708,0,1.5708,base_link,0,0,1,200,5,0,0,,,,,,,,
|
||||||
|
LF_ZHOU_LINK,0.0225,3.63799268487952E-13,-0.00312828322559292,0,0,0,0.0430066968458608,1.55414746498561E-06,-6.51958766162213E-15,6.04358835046556E-23,1.31332156421398E-05,8.81260069658944E-18,1.46158600796761E-05,0,0,0,0,0,0,package://gou2/meshes/LF_ZHOU_LINK.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://gou2/meshes/LF_ZHOU_LINK.STL,,Gou-2-Datui-1,坐标系3,基准轴3,LF_ZHOU_JOINT,revolute,0,0,0,1.5708,0,1.5708,LF_JIAN_LINK,0,0,-1,200,5,-1.5708,1.5708,,,,,,,,
|
||||||
|
LF_XI_LINK,0.00701881640631829,-6.48026486714873E-05,-0.0225280710035354,0,0,0,0.070947069333853,4.48510971806086E-06,1.10821379951271E-07,3.43509633718089E-07,1.05267854653431E-05,6.74865017311917E-15,8.3536065276885E-06,0,0,0,0,0,0,package://gou2/meshes/LF_XI_LINK.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://gou2/meshes/LF_XI_LINK.STL,,Gou-2-Xiaotui-1,坐标系4,基准轴4,LF_XI_JOINT,revolute,0.045,0,0,0,0,-1.5707963267949,LF_ZHOU_LINK,0,0,1,200,5,-1.5708,1.5708,,,,,,,,
|
||||||
|
LF_R_LINK,-0.107342545448481,-0.0330155795013137,-0.0539657436881796,0,0,0,1.39729963569281,0.00190255271211888,-1.28385425078764E-05,1.42747619958461E-06,0.00534215746371847,2.64159261572496E-06,0.00458289280415563,0,0,0,0,0,0,package://gou2/meshes/LF_R_LINK.STL,0.529411764705882,0.549019607843137,0.549019607843137,1,0,0,0,0,0,0,package://gou2/meshes/LF_R_LINK.STL,,Gou-2-xuniguanjie-3,坐标系5,基准轴5,LF_R_JOINT,revolute,0.048,0,0,0,0,0,LF_XI_LINK,0,0,1,200,5,0,0,,,,,,,,
|
||||||
|
LF_P_LINK,-0.107342545448481,-0.0539657436881796,0.0330155795013137,0,0,0,1.39729963569281,0.00190255271211888,1.42747619958462E-06,1.28385425078765E-05,0.00458289280415563,-2.64159261572506E-06,0.00534215746371847,0,0,0,0,0,0,package://gou2/meshes/LF_P_LINK.STL,0.529411764705882,0.549019607843137,0.549019607843137,1,0,0,0,0,0,0,package://gou2/meshes/LF_P_LINK.STL,,Gou-2-xuniguanjie-4,坐标系6,基准轴6,LF_P_JOINT,revolute,0,0,0,1.5707963267949,0,0,LF_R_LINK,0,0,-1,200,5,0,0,,,,,,,,
|
||||||
|
LF_Y_LINK,-0.0330155795013137,-0.0539657436881796,-0.107342545448481,0,0,0,1.39729963569281,0.00534215746371847,2.64159261572502E-06,-1.28385425078765E-05,0.00458289280415563,1.42747619958462E-06,0.00190255271211888,0,0,0,0,0,0,package://gou2/meshes/LF_Y_LINK.STL,0.529411764705882,0.549019607843137,0.549019607843137,1,0,0,0,0,0,0,package://gou2/meshes/LF_Y_LINK.STL,,Gou-2-xuniguanjie-5,坐标系7,基准轴7,LF_Y_JOINT,revolute,0,0,0,0,1.5707963267949,0,LF_P_LINK,0,0,-1,200,5,0,0,,,,,,,,
|
||||||
|
LB_JIAN_LINK,-0.0539657436881803,0.0119844204986863,-0.0654124545515188,0,0,0,1.39729963569281,0.00458289280415563,2.64159261572497E-06,-1.42747619958461E-06,0.00534215746371847,1.28385425078758E-05,0.00190255271211888,0,0,0,0,0,0,package://gou2/meshes/LB_JIAN_LINK.STL,0.529411764705882,0.549019607843137,0.549019607843137,1,0,0,0,0,0,0,package://gou2/meshes/LB_JIAN_LINK.STL,,Gou-2-xuniguanjie-2,坐标系8,基准轴8,LB_JIAN_JOINT,revolute,-0.062375,0.0541,0,-1.5708,0,1.5708,base_link,0,0,1,200,5,0,0,,,,,,,,
|
||||||
|
LB_ZHOU_LINK,0.0225,3.63799268487952E-13,-0.00312828322559291,0,0,0,0.0430066968458608,1.5541474649856E-06,-6.51958590094066E-15,-1.87149613590123E-23,1.31332156421398E-05,8.81256073625921E-18,1.46158600796761E-05,0,0,0,0,0,0,package://gou2/meshes/LB_ZHOU_LINK.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://gou2/meshes/LB_ZHOU_LINK.STL,,Gou-2-Datui-2,坐标系9,基准轴9,LB_ZHOU_JOINT,revolute,0,0,0,1.5707963267949,0,1.5707963267949,LB_JIAN_LINK,0,0,-1,200,5,-1.5708,1.5708,,,,,,,,
|
||||||
|
LB_XI_LINK,0.00701881640631828,-6.48026486715012E-05,-0.0225280710035354,0,0,0,0.070947069333853,4.48510971806086E-06,1.10821379951271E-07,3.43509633718089E-07,1.05267854653431E-05,6.74865074870557E-15,8.35360652768851E-06,0,0,0,0,0,0,package://gou2/meshes/LB_XI_LINK.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://gou2/meshes/LB_XI_LINK.STL,,Gou-2-Xiaotui-2,坐标系10,基准轴10,LB_XI_JOINT,revolute,0.045,0,0,0,0,-1.5707963267949,LB_ZHOU_LINK,0,0,-1,200,5,-1.5708,1.5708,,,,,,,,
|
||||||
|
LB_R_LINK,0.0174074545515188,-0.0330155795013137,-0.0539657436881803,0,0,0,1.39729963569281,0.00190255271211888,-1.28385425078764E-05,1.42747619958458E-06,0.00534215746371847,2.64159261572492E-06,0.00458289280415562,0,0,0,0,0,0,package://gou2/meshes/LB_R_LINK.STL,0.529411764705882,0.549019607843137,0.549019607843137,1,0,0,0,0,0,0,package://gou2/meshes/LB_R_LINK.STL,,Gou-2-xuniguanjie-6,坐标系11,基准轴11,LB_R_JOINT,revolute,0.048,0,0,0,0,0,LB_XI_LINK,0,0,1,200,5,0,0,,,,,,,,
|
||||||
|
LB_P_LINK,0.0174074545515188,-0.0539657436881803,0.0330155795013137,0,0,0,1.39729963569281,0.00190255271211888,1.42747619958464E-06,1.28385425078759E-05,0.00458289280415563,-2.6415926157249E-06,0.00534215746371847,0,0,0,0,0,0,package://gou2/meshes/LB_P_LINK.STL,0.529411764705882,0.549019607843137,0.549019607843137,1,0,0,0,0,0,0,package://gou2/meshes/LB_P_LINK.STL,,Gou-2-xuniguanjie-7,坐标系12,基准轴12,LB_P_JOINT,revolute,0,0,0,1.5707963267949,0,0,LB_R_LINK,0,0,-1,200,5,0,0,,,,,,,,
|
||||||
|
LB_Y_LINK,-0.0330155795013137,-0.0539657436881803,0.0174074545515188,0,0,0,1.39729963569281,0.00534215746371847,2.64159261572499E-06,-1.28385425078765E-05,0.00458289280415563,1.42747619958456E-06,0.00190255271211888,0,0,0,0,0,0,package://gou2/meshes/LB_Y_LINK.STL,0.529411764705882,0.549019607843137,0.549019607843137,1,0,0,0,0,0,0,package://gou2/meshes/LB_Y_LINK.STL,,Gou-2-xuniguanjie-8,坐标系13,基准轴13,LB_Y_JOINT,revolute,0,0,0,0,1.5707963267949,0,LB_P_LINK,0,0,-1,200,5,0,0,,,,,,,,
|
||||||
|
RF_JIAN_LINK,0.0119844204986863,0.0542342563118205,-0.0593375454484812,0,0,0,1.39729963569281,0.00534215746371847,2.64159261572504E-06,-1.2838542507876E-05,0.00458289280415563,1.42747619958461E-06,0.00190255271211888,0,0,0,0,0,0,package://gou2/meshes/RF_JIAN_LINK.STL,0.529411764705882,0.549019607843137,0.549019607843137,1,0,0,0,0,0,0,package://gou2/meshes/RF_JIAN_LINK.STL,,Gou-2-xuniguanjie-9,坐标系14,基准轴14,RF_JIAN_JOINT,revolute,0.062375,-0.0541,0,0,1.5708,0,base_link,0,0,-1,200,5,0,0,,,,,,,,
|
||||||
|
RF_ZHOU_LINK,0.0225,-3.63792329594048E-13,0.00312828322559291,0,0,0,0.0430066968458608,1.5541474649856E-06,6.51958916025171E-15,1.33382873757665E-23,1.31332156421398E-05,8.8125460884089E-18,1.46158600796761E-05,0,0,0,0,0,0,package://gou2/meshes/RF_ZHOU_LINK.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://gou2/meshes/RF_ZHOU_LINK.STL,,Gou-2-Datui-3,坐标系15,基准轴15,RF_ZHOU_JOINT,revolute,0,0,0,-1.5707963267949,0,0,RF_JIAN_LINK,0,0,1,200,5,-1.5708,1.5708,,,,,,,,
|
||||||
|
RF_XI_LINK,0.00702381640631823,-6.48025064629645E-05,0.0225280710035354,0,0,0,0.0709470693338529,4.48510971806086E-06,1.1082011169348E-07,-3.4350963371809E-07,1.05267854653431E-05,6.74865026429312E-15,8.3536065276885E-06,0,0,0,0,0,0,package://gou2/meshes/RF_XI_LINK.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://gou2/meshes/RF_XI_LINK.STL,,Gou-2-Xiaotui2-2,坐标系16,基准轴16,RF_XI_JOINT,revolute,0.045,0,0,0,0,-1.5707963267949,RF_ZHOU_LINK,0,0,-1,200,5,-1.5708,1.5708,,,,,,,,
|
||||||
|
RF_R_LINK,-0.107342545448481,-0.0330155795013137,0.0542342563118205,0,0,0,1.39729963569281,0.00190255271211888,-1.28385425078765E-05,1.42747619958464E-06,0.00534215746371847,2.64159261572492E-06,0.00458289280415563,0,0,0,0,0,0,package://gou2/meshes/RF_R_LINK.STL,0.529411764705882,0.549019607843137,0.549019607843137,1,0,0,0,0,0,0,package://gou2/meshes/RF_R_LINK.STL,,Gou-2-xuniguanjie-10,坐标系17,基准轴17,RF_R_JOINT,revolute,0.048005,0,0,0,0,0,RF_XI_LINK,0,0,1,200,5,0,0,,,,,,,,
|
||||||
|
RF_P_LINK,-0.107342545448481,0.0542342563118205,0.0330155795013138,0,0,0,1.39729963569281,0.00190255271211888,1.42747619958466E-06,1.28385425078765E-05,0.00458289280415563,-2.6415926157249E-06,0.00534215746371847,0,0,0,0,0,0,package://gou2/meshes/RF_P_LINK.STL,0.529411764705882,0.549019607843137,0.549019607843137,1,0,0,0,0,0,0,package://gou2/meshes/RF_P_LINK.STL,,Gou-2-xuniguanjie-11,坐标系18,基准轴18,RF_P_JOINT,revolute,0,0,0,1.5707963267949,0,0,RF_R_LINK,0,0,-1,200,5,0,0,,,,,,,,
|
||||||
|
RF_Y_LINK,-0.0330155795013137,0.0542342563118205,-0.107342545448481,0,0,0,1.39729963569281,0.00534215746371847,2.64159261572496E-06,-1.28385425078765E-05,0.00458289280415563,1.42747619958458E-06,0.00190255271211888,0,0,0,0,0,0,package://gou2/meshes/RF_Y_LINK.STL,0.529411764705882,0.549019607843137,0.549019607843137,1,0,0,0,0,0,0,package://gou2/meshes/RF_Y_LINK.STL,,Gou-2-xuniguanjie-12,坐标系19,基准轴19,RF_Y_JOINT,revolute,0,0,0,0,1.5707963267949,0,RF_P_LINK,0,0,-1,200,5,0,0,,,,,,,,
|
||||||
|
RB_JIAN_LINK,0.0119844204986863,0.0542342563118199,0.0654124545515188,0,0,0,1.39729963569281,0.00534215746371847,2.64159261572502E-06,-1.28385425078759E-05,0.00458289280415563,1.42747619958458E-06,0.00190255271211888,0,0,0,0,0,0,package://gou2/meshes/RB_JIAN_LINK.STL,0.529411764705882,0.549019607843137,0.549019607843137,1,0,0,0,0,0,0,package://gou2/meshes/RB_JIAN_LINK.STL,,Gou-2-xuniguanjie-13,坐标系20,基准轴20,RB_JIAN_JOINT,revolute,-0.0623749999999991,-0.0541,0,0,1.5707963267949,0,base_link,0,0,-1,200,5,0,0,,,,,,,,
|
||||||
|
RB_ZHOU_LINK,0.0225,-3.63785390700144E-13,0.00312828322559291,0,0,0,0.0430066968458608,1.5541474649856E-06,6.51958527968569E-15,1.27744445563994E-22,1.31332156421398E-05,8.81258396622182E-18,1.46158600796761E-05,0,0,0,0,0,0,package://gou2/meshes/RB_ZHOU_LINK.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://gou2/meshes/RB_ZHOU_LINK.STL,,Gou-2-Datui-4,坐标系21,基准轴21,RB_ZHOU_JOINT,revolute,0,0,0,-1.5707963267949,0,0,RB_JIAN_LINK,0,0,1,200,5,-1.5708,1.5708,,,,,,,,
|
||||||
|
RB_XI_LINK,0.00701881640631828,-6.48025064629507E-05,0.0225280710035354,0,0,0,0.0709470693338529,4.48510971806086E-06,1.10820111693481E-07,-3.43509633718088E-07,1.05267854653431E-05,6.74865134818993E-15,8.3536065276885E-06,0,0,0,0,0,0,package://gou2/meshes/RB_XI_LINK.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://gou2/meshes/RB_XI_LINK.STL,,Gou-2-Xiaotui2-1,坐标系22,基准轴22,RB_XI_JOINT,revolute,0.045,0,0,0,0,-1.5707963267949,RB_ZHOU_LINK,0,0,1,200,5,-1.5708,1.5708,,,,,,,,
|
||||||
|
RB_R_LINK,0.0174074545515188,-0.0330155795013137,0.0542342563118199,0,0,0,1.39729963569281,0.00190255271211888,-1.28385425078765E-05,1.4274761995846E-06,0.00534215746371847,2.64159261572506E-06,0.00458289280415563,0,0,0,0,0,0,package://gou2/meshes/RB_R_LINK.STL,0.529411764705882,0.549019607843137,0.549019607843137,1,0,0,0,0,0,0,package://gou2/meshes/RB_R_LINK.STL,,Gou-2-xuniguanjie-14,坐标系23,基准轴23,RB_R_JOINT,revolute,0.048,0,0,0,0,0,RB_XI_LINK,0,0,1,200,5,0,0,,,,,,,,
|
||||||
|
RB_P_LINK,0.0174074545515188,0.0542342563118199,0.0330155795013137,0,0,0,1.39729963569281,0.00190255271211888,1.42747619958453E-06,1.28385425078764E-05,0.00458289280415563,-2.64159261572502E-06,0.00534215746371847,0,0,0,0,0,0,package://gou2/meshes/RB_P_LINK.STL,0.529411764705882,0.549019607843137,0.549019607843137,1,0,0,0,0,0,0,package://gou2/meshes/RB_P_LINK.STL,,Gou-2-xuniguanjie-15,坐标系24,基准轴24,RB_P_JOINT,revolute,0,0,0,1.5707963267949,0,0,RB_R_LINK,0,0,-1,200,5,0,0,,,,,,,,
|
||||||
|
RB_Y_LINK,-0.0330155795013137,0.0542342563118199,0.0174074545515188,0,0,0,1.39729963569281,0.00534215746371847,2.64159261572502E-06,-1.28385425078765E-05,0.00458289280415563,1.42747619958462E-06,0.00190255271211888,0,0,0,0,0,0,package://gou2/meshes/RB_Y_LINK.STL,0.529411764705882,0.549019607843137,0.549019607843137,1,0,0,0,0,0,0,package://gou2/meshes/RB_Y_LINK.STL,,Gou-2-xuniguanjie-16,坐标系25,基准轴25,RB_Y_LINK,revolute,0,0,0,0,1.5707963267949,0,RB_P_LINK,0,0,-1,200,5,0,0,,,,,,,,
|
||||||
|
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue