170901 ROS 세미나 자료가 업데이트 되었다
https://github.com/robotpilot/ros-seminar
13_매니퓰레이터.pdf
https://github.com/ROBOTIS-GIT/open_manipulator
https://github.com/ROBOTIS-GIT/open_manipulator/wiki
open_manipulator 예제가 추가되면서 다이나믹셀 기반 매니퓰레이터 제어 예제가 생겼다
OpenManipulator 및 MoveIt! 의존성 패키지 설치
$ sudo apt-get install ros-kinetic-desktop-full
$ sudo apt-get install ros-kinetic-rqt*
$ sudo apt-get install ros-kinetic-moveit*
$ sudo apt-get install ros-kinetic-industrial-core
$ sudo apt-get install ros-kinetic-dynamixel-sdk
$ sudo apt-get install ros-kinetic-dynamixel-workbench-toolbox #이거실행하지 말고
$ sudo apt-get install ros-kinetic-robotis-math
$ cd ~/catkin_ws/src
$ git clone https://github.com/ROBOTIS-GIT/open_manipulator.git
$ cd ~/catkin_ws && catkin_make
==> 에러가 난다
CMakeFiles/dynamixel_controller.dir/src/dynamixel_controller.cpp.o: In function `open_manipulator_dynamixel_controller::DynamixelController::DynamixelController()':
dynamixel_controller.cpp:(.text+0x2aa): undefined reference to `dynamixel_multi_driver::DynamixelMultiDriver::initSyncWrite()'
dynamixel_controller.cpp:(.text+0x3cf): undefined reference to `dynamixel_multi_driver::DynamixelMultiDriver::initSyncRead()'
/home/bluesky/catkin_ws/src/open_manipulator/open_manipulator_dynamixel_ctrl/include/open_manipulator_dynamixel_ctrl/dynamixel_controller.h
위 파일 내용중에
#include <dynamixel_workbench_toolbox/dynamixel_multi_driver.h>
가 링크가 안되서 발생하는 에러, 실제로 이 파일이 없다.
dynamixel_workbench_toolbox 폴더에 dynamixel_multi_driver 가 없어서 발생하는 문제 였다
/opt/ros/kinetic/include/dynamixel_workbench_toolbox/
위 폴더에 보면 파일이 dynamixel_tool.h 파일 하나만 있다
인터넷을 찾아보니 아래 주소에 dynamixel_multi_driver.h 파일이 있다
https://github.com/ROBOTIS-GIT/dynamixel-workbench
https://github.com/ROBOTIS-GIT/dynamixel-workbench/tree/master/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox
최신 GIT 에는 dynamixel_multi_driver.h가 있는데,
sudo apt-get install ros-kinetic-dynamixel-workbench-toolbox 로 설치했을때는 없다는게 문제점
워크벤치 툴박스 삭제 명령어 하고 재설치 해도 똑같다
sudo apt-get purge ros-kinetic-dynamixel-workbench-toolbox
sudo apt-get upgrade 해도 문제 해결은 안됐다.
==> 해결방법
1) 먼저 설치했던 워크벤치 툴박스를 지우고
sudo apt-get purge ros-kinetic-dynamixel-workbench-toolbox
2) catkin_ws/src/ 에서
git clone https://github.com/ROBOTIS-GIT/dynamixel-workbench.git
git clone https://github.com/ROBOTIS-GIT/dynamixel-workbench-msgs.git
혹시 qt 에러 나면
sudo apt-get install ros-kinetic-qt-build
3)컴파일하면 성공한다
cd ~/catkin_ws && catkin_make
OpenManipulator 모델링 및 동작 테스트
$ roslaunch open_manipulator_description open_manipulator_chain_rviz.launch
URDF 만들고
MoveIt으로 패키지 만들어서 그걸 사용하면 된다
로모 한쪽 팔 URDF 만들고 MoveIt 으로 패키지 생성했는데, 간섭때문인지 동작범위가 매우 작다
testbot.urdf로모 오른쪽팔 URDF 모델링
(------------------------------------------)
ROS 다이나믹셀 제어용 패키지가 나왔다
http://wiki.ros.org/dynamixel_workbench
터미널창에서 다이나믹셀 하나를 셋팅하는 패키지
http://wiki.ros.org/dynamixel_workbench_single_manager/Tutorials/CommandLine
(주의) launch 파일 만들때 프로토콜을 1.0 으로 해야 동작한다.
<arg name="protocol_version" default="1.0"/>GUI 로 다이나믹셀 하나를 설정하는 패키지
http://wiki.ros.org/dynamixel_workbench_single_manager/Tutorials/GUI
(주의) ID, 동작모드, 통신속도를 설정하고 아랫쪽에 torque on 을 눌러야 명령이 먹힌다
(주의) 숫자를 한글자씩 넣을때마다 다이나믹셀 RAM이 업데이트 된다
다이나믹셀 2개로 팬,틸트 위치제어 하는 예제
http://wiki.ros.org/dynamixel_workbench_controllers/Tutorials/PositionControl
현재 MX-106 은 인식이 되는데 EX-106이 미인식이라서 pan,tilt 제어는 못해봄
ROBOTIS 싸이트에서 RoboPlus 라는 SW를 다운받아서
USB2Dynamixel 의 펌웨어를 모두 최신버전으로 업데이트함
그리고 나서 Pan, Tilt 정상 동작함
==> 갖고 있는 EX-106중에 하나는 고장났는지 ROS로 제어도 안되고, RoboPlus SW로도 인식이 안됨. 제품 자체 불량인듯. 다른 EX106은 잘 된다.
최종적으로 여러개 다이나믹셀을 한번에 동작시키는 드라이버
http://wiki.ros.org/dynamixel_workbench_toolbox
## open manipulator 예제를 실행함
https://github.com/ROBOTIS-GIT/open_manipulator
사용법 (wiki page)
https://github.com/ROBOTIS-GIT/open_manipulator/wiki/OpenManipulator-Chain#software-setup
예제에 나온 내용중에 launch 파일을 수정
<arg name="device_name" default="/dev/ttyUSB0"/>
<arg name="baud_rate" default="57600"/>
<arg name="protocol_version" default="1.0"/>
혹시
"Sync Read Failed!" 에러가 난다면..
Dynamixel_controller.cpp 에서
bool DynamixelController::readDynamixelState(void) 함수에서
if (!multi_driver_->syncReadPosition(readValue_->pos))
ROS_ERROR("Sync Read Failed!");
이부분 에러이므로, readDynamixelState 함수를 호출하는 아래 부분을 주석처리 한다
bool DynamixelController::control_loop()
{
// Read & Publish Dynamixel position
//readDynamixelState(); //이부분이 원래 살아 있었는데, 주석처리했다
}
참고로 dynamixel_controller.h 에서
#define MOTOR (0) //0
#define MAX_DXL_NUM (3) //5
#define ITERATION_FREQUENCY (25) //25
이부분에서 연결된 모터의 개수와, 업데이트 주기를 바꿀 수 있고
업데이트 주기가 빠르면 동작이 좀 더 세분화되서 움직인다
==> GIR ROBOTIS issue를 통해 문의해서 원인을 찾았다
sync_read, sync_write 는 다이나믹셀 통신 프로토콜 2.0에서 지원하는 명령어 임
프로토콜 2.0은 MX시리즈 펌웨어 39번 이후로 지원하는데,
우리는 EX-106모터도 있기 때문에 전체 모터를 제어하려면 프로토콜 1.0을 써야 함
로보티즈 담당자 해결책은
syncRead 대신에 프로토콜 1.0에서 모터의 상태를 읽어오는 readRegister 함수를 써야 한다네요.
로보티즈에서 이 부분을 업데이트 해준다고 하니 기다려 보면 될것 같습니다.
https://github.com/ROBOTIS-GIT/open_manipulator/issues/9
[gripper on/off] 그리퍼만 열고 닫는 토픽 명령 송출
rostopic pub /robotis/open_manipulator/gripper std_msgs/String "data: 'grip_on'" --once
[dynamixel control] 하는 런치 파일
roslaunch open_manipulator_dynamixel_ctrl dynamixel_controller.launch
[dynalmixel control by MoveIt] 무브잇 으로 엔드포인트 지정하고 Plan&Execute 누르는 런치파일
roslaunch open_manipulator_moveit open_manipulator_demo.launch
romo_launch 파일 수정중
roslaunch romo_arm demo.launch
romo.urdf 기반으로 moveit 을 통해 만든 패키지에 있는 demo.launch 와
로보티즈가 배포한 open_manipulator_moveit 패키지의 demo.launch 내용이 비슷하다.
즉 moveit으로 패키지를 만든건 동일하고, 로보티즈 예제는 여기에 다이나믹셀을 움직이는 부분을 추가한듯!
demo.launch 파일의 차이점을 찾아보니 아래 내용이 로보티즈 launch 파일에 추가되어 있음
<node name="robotis_joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="use_gui" value="false"/>
<rosparam param="source_list" if="$(arg use_gazebo)">["open_manipulator_chain/joint_states"]</rosparam>
<rosparam param="source_list" unless="$(arg use_gazebo)">["robotis/open_manipulator/present_joint_states"]
</rosparam>
</node>
<!-- Open-Manipulator Chain Topic Pub-->
<node name="open_manipulator_rviz_pub" pkg="open_manipulator_description"
type="open_manipulator_chain_rviz_pub"/>
<!-- Open-Manipultor Position controller -->
<include file="$(find open_manipulator_position_ctrl)/launch/position_ctrl.launch">
<arg name="use_gazebo" value="$(arg use_gazebo)"/>
</include>
즉, 원래 moveit 이 만들어 주는 fake joint로 시뮬레이션 하는 부분까지는 동일하고,
그 이후에 open_manipulator_position_ctrl 패키지를 이용해서 다이나믹셀 모터를 제어하고 있는듯 하다
근데 open_manipulator_position_ctrl을 romo.launch에 그대로 가져다 쓰면 실행되지 않고 닫힌다.
이 패키지를 romo에 맞게 수정해야 하는듯 싶다.
<170917> PAN-TILT 예제로 rad와 절대값 사이의 관계 알아보기
(실행) roslaunch my_dynamixel_workbench_tutorial position_control.launch
dynamixel_state:
-
model_name: EX_106
id: 1
torque_enable: 1
goal_current: 0
goal_velocity: 0
goal_position: 1396
present_current: 0
present_velocity: 0
present_position: 1397
moving: 0
-
model_name: MX_64
id: 2
torque_enable: 1
goal_current: 0
goal_velocity: 0
goal_position: 1396
present_current: 0
present_velocity: 0
present_position: 1397
moving: 0
(명령)
rosservice call /joint_command -- [unit] [pan_pos] [tilt_pos]
bluesky@IDEA2017:~$ rosservice call /joint_command -- rad 0.0 0.0
pan_pos: 2048.0
tilt_pos: 2048.0
bluesky@IDEA2017:~$ rosservice call /joint_command -- rad 1.0 1.0
pan_pos: 2699.0
tilt_pos: 2699.0
bluesky@IDEA2017:~$ rosservice call /joint_command -- rad -1.0 -1.0
pan_pos: 1396.0
tilt_pos: 1396.0
모터축을 정면에서 봐라 봤을때 (+)rad 방향이(raw값 증가) 반시계 방향으로 회전
모터 뒤에서 바라보았을때 (-)rad 방향이(raw값 감소) 반시계 방향으로 회전
http://www.rapidtables.com/convert/number/degrees-to-radians.htm
1 pi = 180[도] = 3.141592[rad]
1[도] = 0.017[rad]
1[rad] = 57.3[도]
bluesky@IDEA2017:~$ rosservice call /joint_command -- rad 3.0 3.0
pan_pos: 4002.0
tilt_pos: 4002.0
bluesky@IDEA2017:~$ rosservice call /joint_command -- rad -3.0 -3.0
pan_pos: 92.0
tilt_pos: 92.0
(명령) raw data angle command
bluesky@IDEA2017:~$ rosrun dynamixel_workbench_operators joint_operator raw 100 100
[ INFO] [1505620580.248663119]: [pan_pos: 100.00 (value)] [tilt_pos: 100.00 (value)]
[ INFO] [1505620598.296576452]: [pan_pos: 4000.00 (value)] [tilt_pos: 4000.00 (value)]
rosservice call /joint_command -- raw 4000.0 4000.0
실제 모터가 회전하는 방향에 맞춰서 오른쪽팔(모터 ID 10번대) URDF 를 수정함
open_manipulator 예제에서 urdf를 romo로 바꾸면 돌아간다
우측팔 아이디를 1~7로 다시 바꿨다. (원래 11~17 이었음)
이렇게 해서 6번 관절까지 늘리면 open_manipulator에 그대로 덮어써서 할 수도 있을듯 싶다
<170920> open_manipulator 5축 모델을 romo에 맞게 7축으로 늘리는 방법
(1) 기존 open_manipulator URDF를 참고해서 URDF를 ROMO에 맞게 7축으로 모델링 한다
주의: joint 마다
<transmission name="tran7">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint7">
....
구문을 추가해야 한다
(2) moveit setup을 실행해서 기존 5축 모델을 7축으로 변경한다
(3) open_manipulator_position_ctrl / src / position_controller.cpp 에서 관절을 추가한다
bool PositionController::initPositionController(void)
{
joint_id_["joint1"] = 1;
joint_id_["joint2"] = 2;
joint_id_["joint3"] = 3;
joint_id_["joint4"] = 4;
joint_id_["joint5"] = 5;
joint_id_["joint6"] = 6; //그립퍼는 제외한다. 그리퍼는 별도로 있다
(4) MAX_JOINT_NUM define을 찾아서 기존 4에서 6으로 늘려준다
open_manipulator_position_ctrl/include/position_controller.h
open_manipulator_position_ctrl/include/motion_planning_tool.h
(5) MAX_DXL_NUM을 찾아서 기존 5에서 7로 늘려 준다
open_manipulator_dynamixel_ctrl/include/dynamixel_controller.h
#define MAX_DXL_NUM (7) //6+1
open_manipulator_chain.srdf 파일에서 zero_pose 중에 joint 4~6이 없어서 기입함
<group_state name="zero_pose" group="arm">
<joint name="joint1" value="0" />
<joint name="joint2" value="-1.5708" />
<joint name="joint3" value="0" />
<joint name="joint4" value="0" />
<joint name="joint5" value="0" />
<joint name="joint6" value="0" />
<joint name="joint7" value="0" />
</group_state>
모션 부드럽게 하는 방법 검색
https://github.com/ros-planning/moveit/issues/416
https://github.com/ros-planning/moveit/issues/160
https://github.com/ros-planning/moveit_tutorials/pull/50/files
팔 두개 모델링?
https://github.com/ros-planning/moveit_core/issues/167
https://github.com/pal-robotics/reem_moveit_config
<170927> 팔이 너무 빨리 움직이는거 해결
planning 시간은 잘 못 가져오는듯 하다
ros::duration 이 시간 클래스는 맞는데..
motion_controller.cpp 에서
motionPlanningTool_->time_from_start_ = motionPlanningTool_->moveit_msg_.trajectory[_tra_index].joint_trajectory.points[_point_index].time_from_start;
함수값을 읽어보면 모두 동일하다.
ROS_INFO("motionPlanningTool_->time_from_start_ %d,%d,%d",_tra_index,_point_index, motionPlanningTool_->time_from_start_ );
[ INFO] [1506523074.226641680]: motionPlanningTool_->points_ 23
[ INFO] [1506523074.226686280]: motionPlanningTool_->time_from_start_ 0,0,18167348
[ INFO] [1506523074.226704732]: motionPlanningTool_->time_from_start_ 0,1,18167348
[ INFO] [1506523074.226723281]: motionPlanningTool_->time_from_start_ 0,2,18167348
[ INFO] [1506523074.226735697]: motionPlanningTool_->time_from_start_ 0,3,18167348
[ INFO] [1506523074.226749105]: motionPlanningTool_->time_from_start_ 0,4,18167348
[ INFO] [1506523074.226761286]: motionPlanningTool_->time_from_start_ 0,5,18167348
[ INFO] [1506523074.226773396]: motionPlanningTool_->time_from_start_ 0,6,18167348
[ INFO] [1506523074.226785781]: motionPlanningTool_->time_from_start_ 0,7,18167348
[ INFO] [1506523074.226798501]: motionPlanningTool_->time_from_start_ 0,8,18167348
[ INFO] [1506523074.226812322]: motionPlanningTool_->time_from_start_ 0,9,18167348
[ INFO] [1506523074.226824717]: motionPlanningTool_->time_from_start_ 0,10,18167348
[ INFO] [1506523074.226837033]: motionPlanningTool_->time_from_start_ 0,11,18167348
[ INFO] [1506523074.226849329]: motionPlanningTool_->time_from_start_ 0,12,18167348
[ INFO] [1506523074.226860496]: motionPlanningTool_->time_from_start_ 0,13,18167348
[ INFO] [1506523074.226872503]: motionPlanningTool_->time_from_start_ 0,14,18167348
[ INFO] [1506523074.226883829]: motionPlanningTool_->time_from_start_ 0,15,18167348
[ INFO] [1506523074.226895576]: motionPlanningTool_->time_from_start_ 0,16,18167348
[ INFO] [1506523074.226910779]: motionPlanningTool_->time_from_start_ 0,17,18167348
[ INFO] [1506523074.226925560]: motionPlanningTool_->time_from_start_ 0,18,18167348
[ INFO] [1506523074.226936986]: motionPlanningTool_->time_from_start_ 0,19,18167348
[ INFO] [1506523074.226948453]: motionPlanningTool_->time_from_start_ 0,20,18167348
[ INFO] [1506523074.226969602]: motionPlanningTool_->time_from_start_ 0,21,18167348
[ INFO] [1506523074.226984802]: motionPlanningTool_->time_from_start_ 0,22,18167348
[ INFO] [1506523074.227000095]: 22 motionPlanningTool_->time_from_start_.toSec() 4891275
하지만 토픽을 직접 읽어보면 time_from_start 값이 증가하는걸 볼 수 있다
rostopic echo /move_group/display_planned_path/trajectory[0]/joint_trajectory
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: /world
joint_names: ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6']
points:
-
positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
accelerations: [-3.8022737011370578e-06, 0.0, 0.0, 0.0, 0.0, 0.0]
effort: []
time_from_start:
secs: 0
nsecs: 0
-
positions: [-0.1315002651835602, -0.0057992475432322626, -0.010564676138417146, 0.0099996833613175, 0.06929653852488736, -0.035217702296038544]
velocities: [-0.0005, -2.2050326420015723e-05, -4.016979024213372e-05, 3.802153306443535e-05, 0.0002634844060130102, -0.00013390734325470152]
accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
effort: []
time_from_start:
secs: 263
nsecs: 530367
-
positions: [-0.2630005303671204, -0.011598495086464525, -0.021129352276834292, 0.019999366722635, 0.13859307704977472, -0.07043540459207709]
velocities: [-0.0005, -2.2050326420015723e-05, -4.016979024213371e-05, 3.802153306443535e-05, 0.00026348440601301013, -0.00013390734325470152]
accelerations: [0.0, 0.0, 7.729562638419933e-23, 0.0, -6.183650110735946e-22, 1.0306083517893243e-22]
effort: []
time_from_start:
secs: 526
nsecs: 1060734
-
positions: [-0.3945007955506806, -0.01739774262969679, -0.031694028415251436, 0.029999050083952504, 0.20788961557466207, -0.10565310688811563]
velocities: [-0.0005, -2.2050326420015726e-05, -4.016979024213372e-05, 3.802153306443535e-05, 0.00026348440601301013, -0.00013390734325470152]
accelerations: [0.0, -1.2882604397366554e-23, -1.2882604397366556e-22, 0.0, 1.0306083517893244e-21, -2.0612167035786486e-22]
effort: []
time_from_start:
secs: 789
nsecs: 1591101
-
positions: [-0.5260010607342408, -0.02319699017292905, -0.042258704553668584, 0.03999873344527, 0.27718615409954944, -0.14087080918415418]
velocities: [-0.0005, -2.2050326420015716e-05, -4.016979024213371e-05, 3.802153306443535e-05, 0.00026348440601301, -0.0001339073432547015]
accelerations: [0.0, 7.72956263841993e-23, 2.3188687915259794e-22, -2.5765208794733105e-23, -1.8550950332207835e-21, 4.1224334071572968e-22]
effort: []
time_from_start:
secs: 1052
nsecs: 2121468
혹시 샘플 코드에서 time_from_start 값을 받아오는 변수 형이 틀려서 그런가 싶어서
motion_planning_tool.h 에 정의된
ros::Duration time_from_start_; // planned movement time
값을 찾았고, duration type을 찾아보니
http://docs.ros.org/diamondback/api/rostime/html/classros_1_1Duration.html
이렇게 되어 있었고
실제 move_group 이 보내는 time_from_start도 type은 duration 이었다.
http://docs.ros.org/jade/api/trajectory_msgs/html/msg/JointTrajectoryPoint.html
toSec 메소드가 값을 동일하게 보여줘서 그런가? 찾아봤더니
http://docs.ros.org/diamondback/api/rostime/html/duration_8h_source.html
double toSec() const { return (double)sec + 1e-9*(double)nsec; };
이렇게 변환하던데
toSec을 쓰면 안된다. 이상하게 toSec으로 변환된 값이 모두 같아 진다 @_@
이거 쓰지 말고 그냥 sec 멤버를 직접 읽는게 방법임
그리고 다이나믹셀의 목표속도를 70 으로 낮춰서 동작시켜야 급속한 움직임을 막을수 있다.
원래 planning 속도 프로파일 자체가 낮은 속도로 되야 하는데, 이상하게 그게 안된다.
그냥 강제로 다이나믹셀의 속도를 낮춰서 일단 임시 대응한다.
<170928> 현재 open_manipulator 폴더 압축해서 업로드
(용량큰 Arduino _gazebo, _with_tb3 폴더는 제외)
open_manipulator 폴더를 만들고 그 밑에서 압축을 해제해야 한다
<170930> 프로그램 시작할 때 모터속도를 느리게 세팅
moving speed 설정 함수는 dynamixel_multi_driver.cpp 에 이미 정의되어 있었음
이걸 이용해서 dynamixel_controller.cpp 에서 명령을 보내면 됨
수정된 코드 업로드 :
open_manipulator_dynamixel_ctrl.tar.gz
open_manipulator 폴더 밑에 있는 open_manipulator_dynamixel_ctrl 폴더를 덮어쓰면 됨
@dunamixel_controller.h
typedef struct
{
std::vector<uint8_t> torque;
std::vector<uint32_t> pos;
std::vector<uint16_t> spd; //add by orasman 170930 //caution: speed declared as uint16 @dynamixel_multi_driver.h
}WriteValue;
@dynamixel_controller.cpp
DynamixelController::DynamixelController(){ //생성자 함수에서 속도값 세팅
....
/added by orasman 170930 모터 동작속도를 낮게 설정
//caution: speed declared as uint16 @dynamixel_multi_driver.h
uint16_t default_moving_speed[MAX_DXL_NUM] = {70,70,70,70,70,70,170};
if (!setMovingSpeed(default_moving_speed) )
ROS_ERROR("Init SyncRead Failed!");
else
ROS_INFO("dynamixel moving speed set succeeded");
...
}
/////////////////////////////////////////
// added by orasman 170930
bool DynamixelController::setMovingSpeed(uint16_t* spd) //caution: speed declared as uint16 @dynamixel_multi_driver.h
{
writeValue_->spd.clear();
for (int id = 1; id <= MAX_DXL_NUM; id++)
{
writeValue_->spd.push_back(spd[id-1]);
}
if (!multi_driver_->syncWriteMovingSpeed(writeValue_->spd))
{
ROS_ERROR("SyncWrite MovingSpeed Failed!");
return false;
}
return true;
}
<EX-106 실제 회전각도와 인식한 각도가 맞지 않는 문제 해결>
수정한 코드 업로드
기존 샘플코드는 EX-106의 경우 value2Radian 변환이 맞지 않는다
즉, 90도를 돌리면 ROS에서 인식한 radian 값이 2.21 정도로 크다 (원래 90도는 1.62rad)
원인은 두가지
dynamixelMultiDriver 에서 dynamixelTool 클래스를 이용해서 모터 정보를 관리하는데
loadDynamixel()할때 모터쪽으로 ping을해서 모터id 와 종류를 받아온다
그래서 multi_dynamixel_[] array 멤버변수에 모터별 특징을 저장하는데
ping을 통해 읽어온 모터 종류번호에 따라 모터 특성이 적혀있는 DB 파일이
EX106,MX106 값이 똑같은게 문제 였음
EX106의 경우 LSB 1비트당 각도가 0.06도이고 최대 회전각도도 0~250.92도 이다
결국 2048을 0rad로 정했으므로 2048은 2.14466058rad 이므로
EX106의 회전 범위는 -2.14466058 ~ +2.14466058 이 되야 한다.
따라서
dynamixel_workbench_toolbox/dynamixel/models/EX/EX_106.device 파일을 수정해야 한다
[device info]
model_number = 107
model_name = EX_106
[type info]
velocity_to_value_ratio = 86.03
value_of_0_radian_position = 2048
value_of_min_radian_position = 0
value_of_max_radian_position = 4095
min_radian = -2.14466058
max_radian = 2.14466058
두번째 원인은
open_manipulator_dunamixel_ctrl 에서
convertValue2Radian 변환 함수에서 모터별 특징을 무시하고 동일하게 계산하는게 문제다
모터 종류별로 min,max rad가 다르므로 모터종류를 받아서 각각 다르게 계산하도록 함수를 수정한다
dynamixel_position.position.push_back(convertValue2Radian( (id-1), (int32_t) multi_driver_->read_value_["present_position"]->at(id-1) ));
float DynamixelController::convertValue2Radian(int id, int32_t value)
{
float radian = 0.0;
if (value > multi_driver_->multi_dynamixel_[id]->value_of_0_radian_position_)
{
if (multi_driver_->multi_dynamixel_[id]->max_radian_ <= 0)
return multi_driver_->multi_dynamixel_[id]->max_radian_;
radian = (float) (value - multi_driver_->multi_dynamixel_[id]->value_of_0_radian_position_) * multi_driver_->multi_dynamixel_[id]->max_radian_
/ (float) (multi_driver_->multi_dynamixel_[id]->value_of_max_radian_position_ - multi_driver_->multi_dynamixel_[id]->value_of_0_radian_position_);
}
else if (value < multi_driver_->multi_dynamixel_[id]->value_of_0_radian_position_)
{
if (multi_driver_->multi_dynamixel_[id]->min_radian_ >= 0)
return multi_driver_->multi_dynamixel_[id]->min_radian_;
radian = (float) (value - multi_driver_->multi_dynamixel_[id]->value_of_0_radian_position_) * multi_driver_->multi_dynamixel_[id]->min_radian_
/ (float) (multi_driver_->multi_dynamixel_[id]->value_of_min_radian_position_ - multi_driver_->multi_dynamixel_[id]->value_of_0_radian_position_);
}
return radian;
}
<MoveIt GUI 없이 로봇팔 괘적 생성하기>
MoveGroupInterface 를 이용해서 moveit을 제어할 수 있다.
http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/pr2_tutorials/planning/src/doc/move_group_interface_tutorial.html
튜토리얼 코드중에 진짜 로봇을 움직이려면 .move() 함수를 쓰라고 나오는데
/* Uncomment below line when working with a real robot */
/* move_group.move() */
찾아보니 이 API가 PLAN&EXECUTE 버튼과 같다고 한다.
https://github.com/ros-planning/moveit_tutorials/issues/75
ㅇㅇㅇ
<171003>
kinect가 publish 하는 관절 정보 받는 방법
http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28C%2B%2B%29
멀티로봇 tf
http://wiki.ros.org/geometry/CoordinateFrameConventions#Transform_Direction
두개의 Origin 에서 기울기 구하는 법을 찾아야 한다. 궁극적으로 Pose가 나와야 한다
https://answers.ros.org/question/10323/from-posestamped-message-to-tf-stampedtransform/
http://docs.ros.org/api/tf/html/c++/transform__datatypes_8h.html
pose 객체는 point와 quaternion 으로 이루어짐
http://docs.ros.org/api/geometry_msgs/html/msg/Pose.html
transformDATAtype method
http://wiki.ros.org/tf/Overview/Using%20Published%20Transforms
http://docs.ros.org/diamondback/api/tf/html/c++/transform__datatypes_8h_source.html
tf::Quarternion class reference
http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1Quaternion.html
<setFromTwoVectors>
https://www.dartdocs.org/documentation/vector_math/1.4.7/vector_math/Vector3-class.html
https://www.dartdocs.org/documentation/vector_math/1.4.7/vector_math/Vector3/Vector3.html
eigen 라이브러리를 이용해서 vector3f 를 만들고 이걸로부터 setFromTwoVectors 를 계산하려고..
https://eigen.tuxfamily.org/dox/group__matrixtypedefs.html#ga5ec9ce2d8adbcd6888f3fbf2e1c095a4
두개의 점이 있는게 아니었다
transform 은 두개의 frame 사이의 변환을 의미한다
즉 origin 이 xyz 떨어진 좌표를 나타내고 rotation이 좌표축의 회전값을 나타내는데,
origin을 방향 벡터로 봐서 이걸로 direction을 구해야 한다
이게제일 유사한데 파이선 코드라서 C++을 찾아야 한다
벡터 한점을 회전으로 변환하는 프로그램
https://answers.ros.org/question/228896/quaternion-of-a-3d-vector/
TF 공부하기
http://wiki.ros.org/tf
＜１７１００６＞
팔길이는 스케일， 오프셋 줘서 대략 맞췄는데，손 방향이 맞지 않아서 고민
팔꿈치～팔 의 방향을 기준으로 Rotation을 만들어야 함
구글링 키워드 ： quaternion between two vectors
https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another
이게 원리고 이걸 쉽게 해주는 함수가 있었다！！
https://stackoverflow.com/questions/15043130/rotation-matrix-in-eigen
이걸 시험삼아 돌려보니 기준벡터에 정한 방향에 맞춰서 두번째 벡터에 연장선에 회전값이 생긴다
quaternion에서 롤피요 앵글 얻는 방법
https://answers.ros.org/question/238083/how-to-get-yaw-from-quaternion-values/
두 벡터 사이의 roll pitch yaw 앵글 구하는 방법
https://stackoverflow.com/questions/39664701/how-to-get-the-angle-pitch-yaw-between-two-3d-vectors-for-an-autoaim
==>이건 한 element가 ０이라서 계산이 안되더라
<171007> 키보드로 실행모드를 지정해주기 (ROMO_MOTOR_ACTIVE 등)
키보드 입력 받는 방법
https://answers.ros.org/question/63491/keyboard-key-pressed/
turtlesim teleop key code 원본 참고
http://docs.ros.org/groovy/api/turtlesim/html/teleop__turtle__key_8cpp_source.html
https://github.com/ros/ros_tutorials/blob/lunar-devel/turtlesim/tutorials/teleop_turtle_key.cpp
토픽으로 메세지 송수신 하는 예제에서 메세지로부터 생성되는 헤더파일은
~/catkin_ws/dev/include/packagename/messagename.h 에 생긴다
ROS message service 생성하는 방법
http://wiki.ros.org/ROS/Tutorials/CreatingMsgAndSrv#Common_step_for_msg_and_srv
<171014>
키넥트로는 손목 각도를 알 수 없어서 moveit에 pose값을 전달할때 손목 각도를 임의로 생성하다 보니
inverse kinematics를 잘 풀지 못하는 경우가 많았다.
myAHRS+ 센서를 구입해서 손목에 장착하면 실제 손목 각도를 측정해서 moveit에 전달할 수 있기때문에
좀더 인간의 자세와 동일한 팔의 모양을 만들 수 있겠다.
https://github.com/robotpilot/myahrs_driver
에 공개된 myAHRS driver를 사용해서 센서값을 읽었다.
왼손 오른손 두개 사용해야 하는데, ttyACM0, ttyACM1 포트만 다르게 한 뒤 노드를 두개 실행하면
두개의 센서값을 읽을 수는 있는데, topic 이름이 동일해서 구별이 안된다.
공개된 소스코드를 복사해서 똑같이 만들고 topic 이름을 다르게 해서 컴파일하고
각각의 노드를 실행하면 왼손, 오른손 각각 다른 이름의 토픽을 publish 하므로 이 값을 이용할 수 있다.
소스코드 업로드
센서는 준비 됐고, 이걸 kinect 가 publish 하는 pose 토픽중 orientation 값으로 변환해서 publish 하면
실제 손의 각도를 표현 할 수 있을것 같다.
kinect 좌표계는 손끝(+z축) 손등(+x축) 손날(+y축) 이므로 myAHRS+ 좌표를 이것에 맞도록 손에 부착하거나
손에 myAHRS+를 편하게 부착한 뒤 좌표계를 kinect 손 모양에 맞도록 변환해 줘야 한다.
IK 를 바로 푸는 예제
라이브러리는 moveit 설치할때 이미 설치되므로 예제만 돌리면 된다.
http://www.orocos.org/kdl/examples
