kenaug74/mycobot_ros
1
0
Fork 0
mirror of https://github.com/elephantrobotics/mycobot_ros.git synced 2026-07-05 19:47:04 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

Merge pull request #82 from elephantrobotics/new_mycobot320_pi

Browse source 
New mycobot320 pi
This commit is contained in:
wangWking 2022-09-14 11:01:02 +08:00 • committed by GitHub
commit 607e1f002b
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
60 changed files with 3150 additions and 1822 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
mycobot_320/mycobot_320/launch/mycobot_320_detect_marker_with_topic.launch
View file

@ -16,7 +16,7 @@
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<!-- mycobot-topics -->
<include file="$(find mycobot_communication)/launch/communication_topic.launch">
<include file="$(find mycobot_320_communication)/launch/communication_topic.launch">
<arg name="port" value="$(arg port)" />
<arg name="baud" value="$(arg baud)" />
</include>

2
mycobot_320/mycobot_320/launch/mycobot_320_simple_gui.launch
View file

@ -13,7 +13,7 @@
<!-- Show in Rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<include file="$(find mycobot_communication)/launch/communication_service.launch">
<include file="$(find mycobot_320_communication)/launch/communication_service.launch">
<arg name="port" value="$(arg port)" />
<arg name="baud" value="$(arg baud)" />
</include>

2
mycobot_320/mycobot_320/launch/mycobot_320_teleop_keyboard.launch
View file

@ -13,7 +13,7 @@
<!-- Show in Rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<include file="$(find mycobot_communication)/launch/communication_service.launch">
<include file="$(find mycobot_320_communication)/launch/communication_service.launch">
<arg name="port" value="$(arg port)" />
<arg name="baud" value="$(arg baud)" />
</include>

6
mycobot_320/mycobot_320/package.xml
View file

@ -49,11 +49,11 @@
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>mycobot_communication</build_depend>
<build_depend>mycobot_320_communication</build_depend>
<build_depend>mycobot_description</build_depend>
<build_export_depend>mycobot_communication</build_export_depend>
<build_export_depend>mycobot_320_communication</build_export_depend>
<build_export_depend>mycobot_description</build_export_depend>
<exec_depend>mycobot_communication</exec_depend>
<exec_depend>mycobot_320_communication</exec_depend>
<exec_depend>mycobot_description</exec_depend>
<exec_depend>std_msgs</exec_depend>
<build_depend>std_msgs</build_depend>

216
mycobot_320/mycobot_320_communication/CMakeLists.txt Normal file
View file

@ -0,0 +1,216 @@
cmake_minimum_required(VERSION 2.8.3)
project(mycobot_320_communication)
add_compile_options(-std=c++11)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
rospy
std_msgs
genmsg
message_generation
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
add_message_files(FILES
MycobotAngles.msg
MycobotCoords.msg
MycobotSetAngles.msg
MycobotSetCoords.msg
MycobotGripperStatus.msg
MycobotPumpStatus.msg
)
## Generate services in the 'srv' folder
add_service_files(FILES
GetAngles.srv
SetAngles.srv
GetCoords.srv
SetCoords.srv
GripperStatus.srv
PumpStatus.srv
)
## Generate added messages and services
generate_messages(DEPENDENCIES std_msgs)
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
## Declare a catkin package
catkin_package(
CATKIN_DEPENDS message_runtime std_msgs
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
# ${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/mycobot_commuication.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/mycobot_commuication_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS
scripts/mycobot_services.py
scripts/mycobot_topics.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_mycobot_commuication.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

25
mycobot_320/mycobot_320_communication/LICENSE Normal file
View file

@ -0,0 +1,25 @@
BSD 2-Clause License
Copyright (c) 2020, Elephant Robotics
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

11
mycobot_320/mycobot_320_communication/launch/communication_jsnn.launch Normal file
View file

@ -0,0 +1,11 @@
<launch>
<!-- Select connecting device and serial port ，选择连接设备及串口-->
<arg name="port" default="/dev/ttyUSB0" />
<arg name="baud" default="115200" />
<!-- Open communication service ，开启通讯服务-->
<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_topics_jsnn.py" output="screen">
<param name="port" type="string" value="$(arg port)" />
<param name="baud" type="int" value="$(arg baud)" />
</node>
</launch>

11
mycobot_320/mycobot_320_communication/launch/communication_seeed.launch Normal file
View file

@ -0,0 +1,11 @@
<launch>
<!-- Select connecting device and serial port ，选择连接设备及串口-->
<arg name="port" default="/dev/ttyUSB0" />
<arg name="baud" default="115200" />
<!-- Open communication service ，开启通讯服务-->
<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_topics_seeed.py" output="screen">
<param name="port" type="string" value="$(arg port)" />
<param name="baud" type="int" value="$(arg baud)" />
</node>
</launch>

11
mycobot_320/mycobot_320_communication/launch/communication_service.launch Normal file
View file

@ -0,0 +1,11 @@
<launch>
<!-- Select connecting device and serial port ，选择连接设备及串口-->
<arg name="port" default="/dev/ttyUSB0" />
<arg name="baud" default="115200" />
<!-- Open communication service ，开启通讯服务-->
<node name="mycobot_services" pkg="mycobot_320_communication" type="mycobot_services.py" output="screen">
<param name="port" type="string" value="$(arg port)" />
<param name="baud" type="int" value="$(arg baud)" />
</node>
</launch>

11
mycobot_320/mycobot_320_communication/launch/communication_service_pi.launch Normal file
View file

@ -0,0 +1,11 @@
<launch>
<!-- Select connecting device and serial port ，选择连接设备及串口-->
<arg name="port" default="/dev/ttyAMA0" />
<arg name="baud" default="115200" />
<!-- Open communication service ，开启通讯服务-->
<node name="mycobot_services" pkg="mycobot_320_communication" type="mycobot_services.py" output="screen">
<param name="port" type="string" value="$(arg port)" />
<param name="baud" type="int" value="$(arg baud)" />
</node>
</launch>

11
mycobot_320/mycobot_320_communication/launch/communication_topic.launch Normal file
View file

@ -0,0 +1,11 @@
<launch>
<!-- Select connecting device and serial port ，选择连接设备及串口-->
<arg name="port" default="/dev/ttyUSB0" />
<arg name="baud" default="115200" />
<!-- Open communication service，开启通讯服务 -->
<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_topics.py" output="screen">
<param name="port" type="string" value="$(arg port)" />
<param name="baud" type="int" value="$(arg baud)" />
</node>
</launch>

11
mycobot_320/mycobot_320_communication/launch/communication_topic_pi.launch Normal file
View file

@ -0,0 +1,11 @@
<launch>
<!-- Select connecting device and serial port ，选择连接设备及串口-->
<arg name="port" default="/dev/ttyUSB0" />
<arg name="baud" default="115200" />
<!-- Open communication service --><!-- 开启通讯服务 -->
<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_topics_pi.py" output="screen">
<param name="port" type="string" value="$(arg port)" />
<param name="baud" type="int" value="$(arg baud)" />
</node>
</launch>

11
mycobot_320/mycobot_320_communication/launch/new_service.launch Normal file
View file

@ -0,0 +1,11 @@
<launch>
<!-- Select connecting device and serial port ，选择连接设备及串口-->
<arg name="port" default="/dev/ttyUSB0" />
<arg name="baud" default="115200" />
<!-- Open communication service ，开启通讯服务-->
<node name="mycobot_services" pkg="mycobot_320_communication" type="mycobot_services.py" output="screen">
<param name="port" type="string" value="$(arg port)" />
<param name="baud" type="int" value="$(arg baud)" />
</node>
</launch>

11
mycobot_320/mycobot_320_communication/launch/new_service_pi.launch Normal file
View file

@ -0,0 +1,11 @@
<launch>
<!-- Select connecting device and serial port ，选择连接设备及串口-->
<arg name="port" default="/dev/ttyAMA0" />
<arg name="baud" default="115200" />
<!-- Open communication service ，开启通讯服务-->
<node name="mycobot_services" pkg="mycobot_320_communication" type="mycobot_services.py" output="screen">
<param name="port" type="string" value="$(arg port)" />
<param name="baud" type="int" value="$(arg baud)" />
</node>
</launch>

6
mycobot_320/mycobot_320_communication/msg/MycobotAngles.msg Normal file
View file

@ -0,0 +1,6 @@
float32 joint_1
float32 joint_2
float32 joint_3
float32 joint_4
float32 joint_5
float32 joint_6

6
mycobot_320/mycobot_320_communication/msg/MycobotCoords.msg Normal file
View file

@ -0,0 +1,6 @@
float32 x
float32 y
float32 z
float32 rx
float32 ry
float32 rz

1
mycobot_320/mycobot_320_communication/msg/MycobotGripperStatus.msg Normal file
View file

@ -0,0 +1 @@
bool Status

3
mycobot_320/mycobot_320_communication/msg/MycobotPumpStatus.msg Normal file
View file

@ -0,0 +1,3 @@
bool Status
int8 Pin1
int8 Pin2

8
mycobot_320/mycobot_320_communication/msg/MycobotSetAngles.msg Normal file
View file

@ -0,0 +1,8 @@
float32 joint_1
float32 joint_2
float32 joint_3
float32 joint_4
float32 joint_5
float32 joint_6
int8 speed

9
mycobot_320/mycobot_320_communication/msg/MycobotSetCoords.msg Normal file
View file

@ -0,0 +1,9 @@
float32 x
float32 y
float32 z
float32 rx
float32 ry
float32 rz
int8 speed
int8 model

66
mycobot_320/mycobot_320_communication/package.xml Normal file
View file

@ -0,0 +1,66 @@
<?xml version="1.0"?>
<package format="2">
<name>mycobot_320_communication</name>
<version>0.1.0</version>
<description>The mycobot_320_commuication package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="lijun.zhang@elephantrobotics.com">zachary</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>BSD</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/mycobot_commuication</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

152
mycobot_320/mycobot_320_communication/scripts/mycobot_services.py Executable file
View file

@ -0,0 +1,152 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import time
import rospy
from mycobot_communication.srv import *
from pymycobot.mycobot import MyCobot
mc = None
def create_handle():
global mc
rospy.init_node("mycobot_services")
rospy.loginfo("start ...")
port = rospy.get_param("~port")
baud = rospy.get_param("~baud")
rospy.loginfo("%s,%s" % (port, baud))
mc = MyCobot(port, baud)
def create_services():
rospy.Service("set_joint_angles", SetAngles, set_angles)
rospy.Service("get_joint_angles", GetAngles, get_angles)
rospy.Service("set_joint_coords", SetCoords, set_coords)
rospy.Service("get_joint_coords", GetCoords, get_coords)
rospy.Service("switch_gripper_status", GripperStatus, switch_status)
rospy.Service("switch_pump_status", PumpStatus, toggle_pump)
rospy.loginfo("ready")
rospy.spin()
def set_angles(req):
"""set angles，设置角度"""
angles = [
req.joint_1,
req.joint_2,
req.joint_3,
req.joint_4,
req.joint_5,
req.joint_6,
]
sp = req.speed
if mc:
mc.send_angles(angles, sp)
return SetAnglesResponse(True)
def get_angles(req):
"""get angles,获取角度"""
if mc:
angles = mc.get_angles()
return GetAnglesResponse(*angles)
def set_coords(req):
coords = [
req.x,
req.y,
req.z,
req.rx,
req.ry,
req.rz,
]
sp = req.speed
mod = req.model
if mc:
mc.send_coords(coords, sp, mod)
return SetCoordsResponse(True)
def get_coords(req):
if mc:
coords = mc.get_coords()
return GetCoordsResponse(*coords)
def switch_status(req):
"""Gripper switch status"""
"""夹爪开关状态"""
if mc:
if req.Status:
mc.set_gripper_state(0, 80)
else:
mc.set_gripper_state(1, 80)
return GripperStatusResponse(True)
def toggle_pump(req):
if mc:
if req.Status:
mc.set_basic_output(req.Pin1, 0)
mc.set_basic_output(req.Pin2, 0)
else:
mc.set_basic_output(req.Pin1, 1)
mc.set_basic_output(req.Pin2, 1)
return PumpStatusResponse(True)
robot_msg = """
MyCobot Status
--------------------------------
Joint Limit:
joint 1: -170 ~ +170
joint 2: -170 ~ +170
joint 3: -170 ~ +170
joint 4: -170 ~ +170
joint 5: -170 ~ +170
joint 6: -180 ~ +180
Connect Status: %s
Servo Infomation: %s
Servo Temperature: %s
Atom Version: %s
"""
def output_robot_message():
connect_status = False
servo_infomation = "unknown"
servo_temperature = "unknown"
atom_version = "unknown"
if mc:
cn = mc.is_controller_connected()
if cn == 1:
connect_status = True
time.sleep(0.1)
si = mc.is_all_servo_enable()
if si == 1:
servo_infomation = "all connected"
print(
robot_msg % (connect_status, servo_infomation,
servo_temperature, atom_version)
)
if __name__ == "__main__":
# print(MyCobot.__dict__)
create_handle()
output_robot_message()
create_services()

216
mycobot_320/mycobot_320_communication/scripts/mycobot_topics.py Executable file
View file

@ -0,0 +1,216 @@
#!/usr/bin/env python2
# -*- coding: utf-8 -*-
import time
import os
import sys
import signal
import threading
import rospy
from mycobot_communication.msg import (
MycobotAngles,
MycobotCoords,
MycobotSetAngles,
MycobotSetCoords,
MycobotGripperStatus,
MycobotPumpStatus,
)
from pymycobot.mycobot import MyCobot
class Watcher:
"""this class solves two problems with multithreaded
programs in Python, (1) a signal might be delivered
to any thread (which is just a malfeature) and (2) if
the thread that gets the signal is waiting, the signal
is ignored (which is a bug).
The watcher is a concurrent process (not thread) that
waits for a signal and the process that contains the
threads. See Appendix A of The Little Book of Semaphores.
http://greenteapress.com/semaphores/
I have only tested this on Linux. I would expect it to
work on the Macintosh and not work on Windows.
"""
def __init__(self):
"""Creates a child thread, which returns. The parent
thread waits for a KeyboardInterrupt and then kills
the child thread.
"""
self.child = os.fork()
if self.child == 0:
return
else:
self.watch()
def watch(self):
try:
os.wait()
except KeyboardInterrupt:
# I put the capital B in KeyBoardInterrupt so I can
# tell when the Watcher gets the SIGINT
print("KeyBoardInterrupt")
self.kill()
sys.exit()
def kill(self):
try:
os.kill(self.child, signal.SIGKILL)
except OSError:
pass
class MycobotTopics(object):
def __init__(self):
super(MycobotTopics, self).__init__()
rospy.init_node("mycobot_topics")
rospy.loginfo("start ...")
port = rospy.get_param("~port", "/dev/ttyUSB0")
baud = rospy.get_param("~baud", 115200)
rospy.loginfo("%s,%s" % (port, baud))
self.mc = MyCobot(port, baud)
self.lock = threading.Lock()
def start(self):
pa = threading.Thread(target=self.pub_real_angles)
pb = threading.Thread(target=self.pub_real_coords)
sa = threading.Thread(target=self.sub_set_angles)
sb = threading.Thread(target=self.sub_set_coords)
sg = threading.Thread(target=self.sub_gripper_status)
sp = threading.Thread(target=self.sub_pump_status)
pa.setDaemon(True)
pa.start()
pb.setDaemon(True)
pb.start()
sa.setDaemon(True)
sa.start()
sb.setDaemon(True)
sb.start()
sg.setDaemon(True)
sg.start()
sp.setDaemon(True)
sp.start()
pa.join()
pb.join()
sa.join()
sb.join()
sg.join()
sp.join()
def pub_real_angles(self):
"""Publish real angle"""
"""发布真实角度"""
pub = rospy.Publisher("mycobot/angles_real",
MycobotAngles, queue_size=5)
ma = MycobotAngles()
while not rospy.is_shutdown():
self.lock.acquire()
angles = self.mc.get_angles()
self.lock.release()
if angles:
ma.joint_1 = angles[0]
ma.joint_2 = angles[1]
ma.joint_3 = angles[2]
ma.joint_4 = angles[3]
ma.joint_5 = angles[4]
ma.joint_6 = angles[5]
pub.publish(ma)
time.sleep(0.25)
def pub_real_coords(self):
"""publish real coordinates"""
"""发布真实坐标"""
pub = rospy.Publisher("mycobot/coords_real",
MycobotCoords, queue_size=5)
ma = MycobotCoords()
while not rospy.is_shutdown():
self.lock.acquire()
coords = self.mc.get_coords()
self.lock.release()
if coords:
ma.x = coords[0]
ma.y = coords[1]
ma.z = coords[2]
ma.rx = coords[3]
ma.ry = coords[4]
ma.rz = coords[5]
pub.publish(ma)
time.sleep(0.25)
def sub_set_angles(self):
"""subscription angles"""
"""订阅角度"""
def callback(data):
angles = [
data.joint_1,
data.joint_2,
data.joint_3,
data.joint_4,
data.joint_5,
data.joint_6,
]
sp = int(data.speed)
self.mc.send_angles(angles, sp)
sub = rospy.Subscriber(
"mycobot/angles_goal", MycobotSetAngles, callback=callback
)
rospy.spin()
def sub_set_coords(self):
def callback(data):
angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz]
sp = int(data.speed)
model = int(data.model)
self.mc.send_coords(angles, sp, model)
sub = rospy.Subscriber(
"mycobot/coords_goal", MycobotSetCoords, callback=callback
)
rospy.spin()
def sub_gripper_status(self):
"""Subscribe to Gripper Status"""
"""订阅夹爪状态"""
def callback(data):
if data.Status:
self.mc.set_gripper_state(0, 80)
else:
self.mc.set_gripper_state(1, 80)
sub = rospy.Subscriber(
"mycobot/gripper_status", MycobotGripperStatus, callback=callback
)
rospy.spin()
def sub_pump_status(self):
def callback(data):
if data.Status:
self.mc.set_basic_output(data.Pin1, 0)
self.mc.set_basic_output(data.Pin2, 0)
else:
self.mc.set_basic_output(data.Pin1, 1)
self.mc.set_basic_output(data.Pin2, 1)
sub = rospy.Subscriber(
"mycobot/pump_status", MycobotPumpStatus, callback=callback
)
rospy.spin()
if __name__ == "__main__":
Watcher()
mc_topics = MycobotTopics()
mc_topics.start()
# while True:
# mc_topics.pub_real_coords()
# mc_topics.sub_set_angles()
pass

224
mycobot_320/mycobot_320_communication/scripts/mycobot_topics_jsnn.py Normal file
View file

@ -0,0 +1,224 @@
#!/usr/bin/env python2
# -*- coding: utf-8 -*-
import time
import os
import sys
import signal
import threading
import rospy
from mycobot_communication.msg import (
MycobotAngles,
MycobotCoords,
MycobotSetAngles,
MycobotSetCoords,
MycobotGripperStatus,
MycobotPumpStatus,
)
# from pymycobot import MyCobot
from pymycobot import MyCobotSocket # pi
class Watcher:
"""this class solves two problems with multithreaded
programs in Python, (1) a signal might be delivered
to any thread (which is just a malfeature) and (2) if
the thread that gets the signal is waiting, the signal
is ignored (which is a bug).
The watcher is a concurrent process (not thread) that
waits for a signal and the process that contains the
threads. See Appendix A of The Little Book of Semaphores.
http://greenteapress.com/semaphores/
I have only tested this on Linux. I would expect it to
work on the Macintosh and not work on Windows.
"""
def __init__(self):
"""Creates a child thread, which returns. The parent
thread waits for a KeyboardInterrupt and then kills
the child thread.
"""
self.child = os.fork()
if self.child == 0:
return
else:
self.watch()
def watch(self):
try:
os.wait()
except KeyboardInterrupt:
# I put the capital B in KeyBoardInterrupt so I can#
# 我把大写的 B 放在 KeyBoardInterrupt 中，这样我就可以了
# tell when the Watcher gets the SIGINT，告诉 Watcher 何时收到 SIGINT
print("KeyBoardInterrupt")
self.kill()
sys.exit()
def kill(self):
try:
os.kill(self.child, signal.SIGKILL)
except OSError:
pass
class MycobotTopics(object):
def __init__(self):
super(MycobotTopics, self).__init__()
rospy.init_node("mycobot_topics")
rospy.loginfo("start ...")
# Select connected device，选择连接设备
port = rospy.get_param("~port", "/dev/ttyUSB0")
baud = rospy.get_param("~baud", 115200)
rospy.loginfo("%s,%s" % (port, baud))
# self.mc = MyCobot(port,baud)
self.mc = MyCobotSocket(port, baud) # port
self.mc.connect()
self.lock = threading.Lock()
def start(self):
pa = threading.Thread(target=self.pub_real_angles)
pb = threading.Thread(target=self.pub_real_coords)
sa = threading.Thread(target=self.sub_set_angles)
sb = threading.Thread(target=self.sub_set_coords)
sg = threading.Thread(target=self.sub_gripper_status)
sp = threading.Thread(target=self.sub_pump_status)
pa.setDaemon(True)
pa.start()
pb.setDaemon(True)
pb.start()
sa.setDaemon(True)
sa.start()
sb.setDaemon(True)
sb.start()
sg.setDaemon(True)
sg.start()
sp.setDaemon(True)
sp.start()
pa.join()
pb.join()
sa.join()
sb.join()
sg.join()
sp.join()
def pub_real_angles(self):
"""Publish real angle"""
"""发布真实角度"""
pub = rospy.Publisher("mycobot/angles_real", MycobotAngles, queue_size=5)
ma = MycobotAngles()
while not rospy.is_shutdown():
self.lock.acquire()
angles = self.mc.get_angles()
self.lock.release()
if angles:
ma.joint_1 = angles[0]
ma.joint_2 = angles[1]
ma.joint_3 = angles[2]
ma.joint_4 = angles[3]
ma.joint_5 = angles[4]
ma.joint_6 = angles[5]
pub.publish(ma)
time.sleep(0.25)
def pub_real_coords(self):
"""publish real coordinates"""
"""发布真实坐标"""
pub = rospy.Publisher("mycobot/coords_real", MycobotCoords, queue_size=5)
ma = MycobotCoords()
while not rospy.is_shutdown():
self.lock.acquire()
coords = self.mc.get_coords()
self.lock.release()
if coords:
ma.x = coords[0]
ma.y = coords[1]
ma.z = coords[2]
ma.rx = coords[3]
ma.ry = coords[4]
ma.rz = coords[5]
pub.publish(ma)
time.sleep(0.25)
def sub_set_angles(self):
"""subscription angles"""
"""订阅角度"""
def callback(data):
angles = [
data.joint_1,
data.joint_2,
data.joint_3,
data.joint_4,
data.joint_5,
data.joint_6,
]
sp = int(data.speed)
self.mc.send_angles(angles, sp)
sub = rospy.Subscriber(
"mycobot/angles_goal", MycobotSetAngles, callback=callback
)
rospy.spin()
def sub_set_coords(self):
"""Subscribe to coordinates"""
"""订阅坐标"""
def callback(data):
angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz]
sp = int(data.speed)
model = int(data.model)
self.mc.send_coords(angles, sp, model)
sub = rospy.Subscriber(
"mycobot/coords_goal", MycobotSetCoords, callback=callback
)
rospy.spin()
def sub_gripper_status(self):
"""Subscribe to Gripper Status"""
"""订阅夹爪状态"""
def callback(data):
if data.Status:
self.mc.set_gripper_state(0, 80)
else:
self.mc.set_gripper_state(1, 80)
sub = rospy.Subscriber(
"mycobot/gripper_status", MycobotGripperStatus, callback=callback
)
rospy.spin()
def sub_pump_status(self):
def callback(data):
if data.Status:
self.mc.set_basic_output(data.Pin1, 0)
self.mc.set_basic_output(data.Pin2, 0)
else:
self.mc.set_basic_output(data.Pin1, 1)
self.mc.set_basic_output(data.Pin2, 1)
sub = rospy.Subscriber(
"mycobot/pump_status", MycobotPumpStatus, callback=callback
)
rospy.spin()
if __name__ == "__main__":
Watcher()
mc_topics = MycobotTopics()
mc_topics.start()
# while True:
# mc_topics.pub_real_coords()
# mc_topics.sub_set_angles()
pass

218
mycobot_320/mycobot_320_communication/scripts/mycobot_topics_pi.py Normal file
View file

@ -0,0 +1,218 @@
#!/usr/bin/env python2
# -*- coding: utf-8 -*-
import time
import os
import sys
import signal
import threading
import rospy
from mycobot_communication.msg import (
MycobotAngles,
MycobotCoords,
MycobotSetAngles,
MycobotSetCoords,
MycobotGripperStatus,
MycobotPumpStatus,
)
from pymycobot import MyCobotSocket
class Watcher:
"""this class solves two problems with multithreaded
programs in Python, (1) a signal might be delivered
to any thread (which is just a malfeature) and (2) if
the thread that gets the signal is waiting, the signal
is ignored (which is a bug).
The watcher is a concurrent process (not thread) that
waits for a signal and the process that contains the
threads. See Appendix A of The Little Book of Semaphores.
http://greenteapress.com/semaphores/
I have only tested this on Linux. I would expect it to
work on the Macintosh and not work on Windows.
"""
def __init__(self):
"""Creates a child thread, which returns. The parent
thread waits for a KeyboardInterrupt and then kills
the child thread.创建一个返回的子线程。 父线程等待 KeyboardInterrupt
然后杀死子线程。
"""
self.child = os.fork()
if self.child == 0:
return
else:
self.watch()
def watch(self):
try:
os.wait()
except KeyboardInterrupt:
# I put the capital B in KeyBoardInterrupt so I can
# tell when the Watcher gets the SIGINT
print("KeyBoardInterrupt")
self.kill()
sys.exit()
def kill(self):
try:
os.kill(self.child, signal.SIGKILL)
except OSError:
pass
class MycobotTopics(object):
def __init__(self):
super(MycobotTopics, self).__init__()
rospy.init_node("mycobot_topics_pi")
rospy.loginfo("start ...")
# problem
port = rospy.get_param("~port", "/dev/ttyUSB0")
baud = rospy.get_param("~baud", 115200)
rospy.loginfo("%s,%s" % (port, baud))
self.mc = MyCobotSocket(port, baud) # port
self.mc.connect() #pi
self.lock = threading.Lock()
def start(self):
pa = threading.Thread(target=self.pub_real_angles)
pb = threading.Thread(target=self.pub_real_coords)
sa = threading.Thread(target=self.sub_set_angles)
sb = threading.Thread(target=self.sub_set_coords)
sg = threading.Thread(target=self.sub_gripper_status)
sp = threading.Thread(target=self.sub_pump_status)
pa.setDaemon(True)
pa.start()
pb.setDaemon(True)
pb.start()
sa.setDaemon(True)
sa.start()
sb.setDaemon(True)
sb.start()
sg.setDaemon(True)
sg.start()
sp.setDaemon(True)
sp.start()
pa.join()
pb.join()
sa.join()
sb.join()
sg.join()
sp.join()
def pub_real_angles(self):
"""Publish real angle"""
"""发布真实角度"""
pub = rospy.Publisher("mycobot/angles_real", MycobotAngles, queue_size=5)
ma = MycobotAngles()
while not rospy.is_shutdown():
self.lock.acquire()
angles = self.mc.get_angles()
self.lock.release()
if angles:
ma.joint_1 = angles[0]
ma.joint_2 = angles[1]
ma.joint_3 = angles[2]
ma.joint_4 = angles[3]
ma.joint_5 = angles[4]
ma.joint_6 = angles[5]
pub.publish(ma)
time.sleep(0.25)
def pub_real_coords(self):
"""publish real coordinates"""
"""发布真实坐标"""
pub = rospy.Publisher("mycobot/coords_real", MycobotCoords, queue_size=5)
ma = MycobotCoords()
while not rospy.is_shutdown():
self.lock.acquire()
coords = self.mc.get_coords()
self.lock.release()
if coords:
ma.x = coords[0]
ma.y = coords[1]
ma.z = coords[2]
ma.rx = coords[3]
ma.ry = coords[4]
ma.rz = coords[5]
pub.publish(ma)
time.sleep(0.25)
def sub_set_angles(self):
"""subscription angles"""
"""订阅角度"""
def callback(data):
angles = [
data.joint_1,
data.joint_2,
data.joint_3,
data.joint_4,
data.joint_5,
data.joint_6,
]
sp = int(data.speed)
self.mc.send_angles(angles, sp)
sub = rospy.Subscriber(
"mycobot/angles_goal", MycobotSetAngles, callback=callback
)
rospy.spin()
def sub_set_coords(self):
def callback(data):
angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz]
sp = int(data.speed)
model = int(data.model)
self.mc.send_coords(angles, sp, model)
sub = rospy.Subscriber(
"mycobot/coords_goal", MycobotSetCoords, callback=callback
)
rospy.spin()
def sub_gripper_status(self):
"""Subscribe to Gripper Status"""
"""订阅夹爪状态"""
def callback(data):
if data.Status:
self.mc.set_gripper_state(0, 80)
else:
self.mc.set_gripper_state(1, 80)
sub = rospy.Subscriber(
"mycobot/gripper_status", MycobotGripperStatus, callback=callback
)
rospy.spin()
def sub_pump_status(self):
def callback(data):
if data.Status:
self.mc.set_basic_output(data.Pin1, 0)
self.mc.set_basic_output(data.Pin2, 0)
else:
self.mc.set_basic_output(data.Pin1, 1)
self.mc.set_basic_output(data.Pin2, 1)
sub = rospy.Subscriber(
"mycobot/pump_status", MycobotPumpStatus, callback=callback
)
rospy.spin()
if __name__ == "__main__":
Watcher()
mc_topics = MycobotTopics()
mc_topics.start()
# while True:
# mc_topics.pub_real_coords()
# mc_topics.sub_set_angles()
pass

215
mycobot_320/mycobot_320_communication/scripts/mycobot_topics_seeed.py Normal file
View file

@ -0,0 +1,215 @@
#!/usr/bin/env python2
# -*- coding: utf-8 -*-
import time
import os
import sys
import signal
import threading
import rospy
from mycobot_communication.msg import (
MycobotAngles,
MycobotCoords,
MycobotSetAngles,
MycobotSetCoords,
MycobotGripperStatus,
MycobotPumpStatus,
)
from pymycobot import MyCobot
class Watcher:
"""this class solves two problems with multithreaded
programs in Python, (1) a signal might be delivered
to any thread (which is just a malfeature) and (2) if
the thread that gets the signal is waiting, the signal
is ignored (which is a bug).
The watcher is a concurrent process (not thread) that
waits for a signal and the process that contains the
threads. See Appendix A of The Little Book of Semaphores.
http://greenteapress.com/semaphores/
I have only tested this on Linux. I would expect it to
work on the Macintosh and not work on Windows.
"""
def __init__(self):
"""Creates a child thread, which returns. The parent
thread waits for a KeyboardInterrupt and then kills
the child thread.
"""
self.child = os.fork()
if self.child == 0:
return
else:
self.watch()
def watch(self):
try:
os.wait()
except KeyboardInterrupt:
# I put the capital B in KeyBoardInterrupt so I can
# tell when the Watcher gets the SIGINT
print("KeyBoardInterrupt")
self.kill()
sys.exit()
def kill(self):
try:
os.kill(self.child, signal.SIGKILL)
except OSError:
pass
class MycobotTopics(object):
def __init__(self):
super(MycobotTopics, self).__init__()
rospy.init_node("mycobot_topics")
rospy.loginfo("start ...")
# problem
port = rospy.get_param("~port", "/dev/ttyUSB0")
baud = rospy.get_param("~baud", 115200)
rospy.loginfo("%s,%s" % (port, baud))
self.mc = MyCobot(port,baud)
self.lock = threading.Lock()
def start(self):
pa = threading.Thread(target=self.pub_real_angles)
pb = threading.Thread(target=self.pub_real_coords)
sa = threading.Thread(target=self.sub_set_angles)
sb = threading.Thread(target=self.sub_set_coords)
sg = threading.Thread(target=self.sub_gripper_status)
sp = threading.Thread(target=self.sub_pump_status)
pa.setDaemon(True)
pa.start()
pb.setDaemon(True)
pb.start()
sa.setDaemon(True)
sa.start()
sb.setDaemon(True)
sb.start()
sg.setDaemon(True)
sg.start()
sp.setDaemon(True)
sp.start()
pa.join()
pb.join()
sa.join()
sb.join()
sg.join()
sp.join()
def pub_real_angles(self):
"""Publish real angle"""
"""发布真实角度"""
pub = rospy.Publisher("mycobot/angles_real", MycobotAngles, queue_size=5)
ma = MycobotAngles()
while not rospy.is_shutdown():
self.lock.acquire()
angles = self.mc.get_angles()
self.lock.release()
if angles:
ma.joint_1 = angles[0]
ma.joint_2 = angles[1]
ma.joint_3 = angles[2]
ma.joint_4 = angles[3]
ma.joint_5 = angles[4]
ma.joint_6 = angles[5]
pub.publish(ma)
time.sleep(0.25)
def pub_real_coords(self):
"""publish real coordinates"""
"""发布真实坐标"""
pub = rospy.Publisher("mycobot/coords_real", MycobotCoords, queue_size=5)
ma = MycobotCoords()
while not rospy.is_shutdown():
self.lock.acquire()
coords = self.mc.get_coords()
self.lock.release()
if coords:
ma.x = coords[0]
ma.y = coords[1]
ma.z = coords[2]
ma.rx = coords[3]
ma.ry = coords[4]
ma.rz = coords[5]
pub.publish(ma)
time.sleep(0.25)
def sub_set_angles(self):
"""subscription angles"""
"""订阅角度"""
def callback(data):
angles = [
data.joint_1,
data.joint_2,
data.joint_3,
data.joint_4,
data.joint_5,
data.joint_6,
]
sp = int(data.speed)
self.mc.send_angles(angles, sp)
sub = rospy.Subscriber(
"mycobot/angles_goal", MycobotSetAngles, callback=callback
)
rospy.spin()
def sub_set_coords(self):
def callback(data):
angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz]
sp = int(data.speed)
model = int(data.model)
self.mc.send_coords(angles, sp, model)
sub = rospy.Subscriber(
"mycobot/coords_goal", MycobotSetCoords, callback=callback
)
rospy.spin()
def sub_gripper_status(self):
"""Subscribe to Gripper Status"""
"""订阅夹爪状态"""
def callback(data):
if data.Status:
self.mc.set_gripper_state(0, 80)
else:
self.mc.set_gripper_state(1, 80)
sub = rospy.Subscriber(
"mycobot/gripper_status", MycobotGripperStatus, callback=callback
)
rospy.spin()
def sub_pump_status(self):
def callback(data):
if data.Status:
self.mc.set_basic_output(data.Pin1, 0)
self.mc.set_basic_output(data.Pin2, 0)
else:
self.mc.set_basic_output(data.Pin1, 1)
self.mc.set_basic_output(data.Pin2, 1)
sub = rospy.Subscriber(
"mycobot/pump_status", MycobotPumpStatus, callback=callback
)
rospy.spin()
if __name__ == "__main__":
Watcher()
mc_topics = MycobotTopics()
mc_topics.start()
# while True:
# mc_topics.pub_real_coords()
# mc_topics.sub_set_angles()
pass

9
mycobot_320/mycobot_320_communication/srv/GetAngles.srv Normal file
View file

@ -0,0 +1,9 @@
---
float32 joint_1
float32 joint_2
float32 joint_3
float32 joint_4
float32 joint_5
float32 joint_6

9
mycobot_320/mycobot_320_communication/srv/GetCoords.srv Normal file
View file

@ -0,0 +1,9 @@
---
float32 x
float32 y
float32 z
float32 rx
float32 ry
float32 rz

5
mycobot_320/mycobot_320_communication/srv/GripperStatus.srv Normal file
View file

@ -0,0 +1,5 @@
bool Status
---
bool Flag

7
mycobot_320/mycobot_320_communication/srv/PumpStatus.srv Normal file
View file

@ -0,0 +1,7 @@
bool Status
int8 Pin1
int8 Pin2
---
bool Flag

12
mycobot_320/mycobot_320_communication/srv/SetAngles.srv Normal file
View file

@ -0,0 +1,12 @@
float32 joint_1
float32 joint_2
float32 joint_3
float32 joint_4
float32 joint_5
float32 joint_6
int8 speed
---
bool Flag

13
mycobot_320/mycobot_320_communication/srv/SetCoords.srv Normal file
View file

@ -0,0 +1,13 @@
float32 x
float32 y
float32 z
float32 rx
float32 ry
float32 rz
int8 speed
int8 model
---
bool Flag

2
mycobot_320/new_mycobot_320/launch/mycobot_320_detect_marker_with_topic.launch
View file

@ -16,7 +16,7 @@
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<!-- mycobot-topics -->
<include file="$(find mycobot_communication)/launch/communication_topic.launch">
<include file="$(find mycobot_320_communication)/launch/communication_topic.launch">
<arg name="port" value="$(arg port)" />
<arg name="baud" value="$(arg baud)" />
</include>

2
mycobot_320/new_mycobot_320/launch/mycobot_320_simple_gui.launch
View file

@ -13,7 +13,7 @@
<!-- Show in Rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<include file="$(find mycobot_communication)/launch/communication_service.launch">
<include file="$(find mycobot_320_communication)/launch/new_service.launch">
<arg name="port" value="$(arg port)" />
<arg name="baud" value="$(arg baud)" />
</include>

2
mycobot_320/new_mycobot_320/launch/mycobot_320_teleop_keyboard.launch
View file

@ -13,7 +13,7 @@
<!-- Show in Rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<include file="$(find mycobot_communication)/launch/communication_service.launch">
<include file="$(find mycobot_320_communication)/launch/new_service.launch">
<arg name="port" value="$(arg port)" />
<arg name="baud" value="$(arg baud)" />
</include>

6
mycobot_320/new_mycobot_320/package.xml
View file

@ -58,11 +58,11 @@
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend> -->
<exec_depend>std_msgs</exec_depend>
<build_depend>mycobot_communication</build_depend>
<build_depend>mycobot_320_communication</build_depend>
<build_depend>mycobot_description</build_depend>
<build_export_depend>mycobot_communication</build_export_depend>
<build_export_depend>mycobot_320_communication</build_export_depend>
<build_export_depend>mycobot_description</build_export_depend>
<exec_depend>mycobot_communication</exec_depend>
<exec_depend>mycobot_320_communication</exec_depend>
<exec_depend>mycobot_description</exec_depend>
<exec_depend>actionlib</exec_depend>
<build_depend>actionlib</build_depend>

6
mycobot_320/new_mycobot_320_pi/config/new_mycobot_320_pi.rviz
View file

@ -122,7 +122,7 @@ Visualization Manager:
Value: true
link6:
Value: true
Marker Scale: 0.300000012
Marker Scale: 0.400000006
Name: TF
Show Arrows: true
Show Axes: true
@ -178,10 +178,10 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.584796429
Pitch: 0.394796789
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 3.21858907
Yaw: 3.10358953
Saved: ~
Window Geometry:
Displays:

2
mycobot_320/new_mycobot_320_pi/launch/mycobot_320_detect_marker_with_topic.launch
View file

@ -16,7 +16,7 @@
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<!-- mycobot-topics -->
<include file="$(find mycobot_communication)/launch/communication_topic.launch">
<include file="$(find mycobot_320_communication)/launch/communication_topic.launch">
<arg name="port" value="$(arg port)" />
<arg name="baud" value="$(arg baud)" />
</include>

2
mycobot_320/new_mycobot_320_pi/launch/mycobot_320_simple_gui.launch
View file

@ -13,7 +13,7 @@
<!-- Show in Rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<include file="$(find mycobot_communication)/launch/communication_service.launch">
<include file="$(find mycobot_320_communication)/launch/new_service_pi.launch">
<arg name="port" value="$(arg port)" />
<arg name="baud" value="$(arg baud)" />
</include>

6
mycobot_320/new_mycobot_320_pi/launch/mycobot_320_slider.launch
View file

@ -6,12 +6,6 @@
<arg name="rvizconfig" default="$(find new_mycobot_320_pi)/config/new_mycobot_320_pi.rviz" />
<arg name="gui" default="true" />
<!-- <include file="$(find mycobot_280)/launch/slider_control.launch">
<arg name="model" value="$(arg model)" />
<arg name="rvizconfig" value="$(arg rvizconfig)" />
<arg name="gui" value="$(arg gui)" />
</include> -->
<!-- new -->
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />

2
mycobot_320/new_mycobot_320_pi/launch/mycobot_320_teleop_keyboard.launch
View file

@ -13,7 +13,7 @@
<!-- Show in Rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<include file="$(find mycobot_communication)/launch/communication_service.launch">
<include file="$(find mycobot_320_communication)/launch/new_service_pi.launch">
<arg name="port" value="$(arg port)" />
<arg name="baud" value="$(arg baud)" />
</include>

2
mycobot_320/new_mycobot_320_pi/launch/mycobot_320_test.launch
View file

@ -8,7 +8,7 @@
<!-- Combinejoin values to TF -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui">
<param name="use_gui" value="$(arg gui)" />
<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
</node>

3
mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_follow_display.py
View file

@ -1,6 +1,5 @@
#!/usr/bin/env python2
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
from cgi import print_environ
import time
import rospy

2
mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_following_marker.py
View file

@ -1,4 +1,4 @@
#!/usr/bin/env python2
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import time

4
mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_listen_real.py
View file

@ -1,4 +1,4 @@
#!/usr/bin/env python2
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# license removed for brevity
import time
@ -28,7 +28,7 @@ def talker():
"joint6_to_joint5",
"joint6output_to_joint6",
]
joint_state_send.velocity = [0]
joint_state_send.velocity = [0.0]
joint_state_send.effort = []
# waiting util server `get_joint_angles` enable.

2
mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_listen_real_of_topic.py
View file

@ -1,4 +1,4 @@
#!/usr/bin/env python2
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import math

6
mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_simple_gui.py
View file

@ -1,6 +1,6 @@
#!/usr/bin/env python
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import Tkinter as tk
import tkinter as tk
from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
import rospy
import time
@ -32,7 +32,7 @@ class Window:
# calculate x and y coordinates for the Tk root window
x = (self.ws / 2) - 190
y = (self.hs / 2) - 250
self.win.geometry("430x370+{}+{}".format(x, y))
self.win.geometry("430x400+{}+{}".format(x, y))
# 布局
self.set_layout()
# 输入部分

5
mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_slider.py
View file

@ -1,4 +1,4 @@
#!/usr/bin/env python2
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""[summary]
@ -7,7 +7,7 @@ and then sends it directly to the real manipulator using `pymycobot` API.
This file is [slider_control.launch] related script.
Passable parameters:
port: serial prot string. Defaults is '/dev/ttyAMA0'
baud: serial prot baudrate. Defaults is 1000000.
baud: serial prot baudrate. Defaults is 115200.
"""
import rospy
@ -28,6 +28,7 @@ def callback(data):
mc.send_radians(data_list, 80)
print('data_list:', data_list)
# time.sleep(0.5)

4
mycobot_320/new_mycobot_320_pi/scripts/mycobot_320_teleop_keyboard.py
View file

@ -1,4 +1,4 @@
#!/usr/bin/env python
#!/usr/bin/env pytho3
# -*- coding: utf-8 -*-
from __future__ import print_function
from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
@ -148,8 +148,10 @@ def teleop_keyboard():
switch_gripper(False)
elif key == "1":
rsp = set_angles(*init_pose)
record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
elif key in "2":
rsp = set_angles(*home_pose)
record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
elif key in "3":
rep = get_angles()
home_pose[0] = rep.joint_1

6
mycobot_320/new_mycobot_320_pi_moveit/launch/moveit.rviz
View file

@ -225,10 +225,10 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.0797968805
Pitch: -0.275203139
Target Frame: base
Value: XYOrbit (rviz)
Yaw: 2.89994884
Yaw: 2.96994972
Saved: ~
Window Geometry:
Displays:
@ -246,5 +246,5 @@ Window Geometry:
Views:
collapsed: false
Width: 1855
X: 58
X: 65
Y: 24

2
mycobot_communication/scripts/mycobot_services.py
View file

@ -1,5 +1,5 @@
#!/usr/bin/env python2
# -*- coding:utf-8 -*-
# -*- coding: utf-8 -*
import time
import rospy
from mycobot_communication.srv import *

863
mycobot_description/urdf/new_320_pi/base.dae Normal file → Executable file
View file

File diff suppressed because one or more lines are too long

112
mycobot_description/urdf/new_320_pi/link1.dae Normal file → Executable file
View file

File diff suppressed because one or more lines are too long

656
mycobot_description/urdf/new_320_pi/link2.dae Normal file → Executable file
View file

File diff suppressed because one or more lines are too long

962
mycobot_description/urdf/new_320_pi/link3.dae Normal file → Executable file
View file

File diff suppressed because one or more lines are too long

146
mycobot_description/urdf/new_320_pi/link4.dae Normal file → Executable file
View file

File diff suppressed because one or more lines are too long

450
mycobot_description/urdf/new_320_pi/link5.dae Normal file → Executable file
View file

File diff suppressed because one or more lines are too long

110
mycobot_description/urdf/new_320_pi/link6.dae Normal file → Executable file
View file

File diff suppressed because one or more lines are too long

45
mycobot_description/urdf/new_320_pi/new_mycobot_pro_320_pi.urdf
View file

@ -27,13 +27,13 @@
<mesh filename="package://mycobot_description/urdf/new_320_pi/link1.dae"/>
</geometry>
<origin xyz = "-0.29156 -0.220 -0.085 " rpy = " 0 0 1.5708"/>
<origin xyz = "0.115 -0.172 -0.086 " rpy = " 0 0 3.1415926"/>
</visual>
<collision>
<geometry>
<mesh filename="package://mycobot_description/urdf/new_320_pi/link1.dae"/>
</geometry>
<origin xyz = "-0.29156 -0.220 -0.085 " rpy = " 0 0 1.5708"/>
<origin xyz = "0.115 -0.172 -0.09 " rpy = " 0 0 3.1415926"/>
</collision>
</link>
@ -44,13 +44,13 @@
<mesh filename="package://mycobot_description/urdf/new_320_pi/link2.dae"/>
</geometry>
<origin xyz = "-0.090 0.0 -0.138 " rpy = " 0 1.5708 0"/>
<origin xyz = "-0.075 -0.115 -0.022 " rpy = " 1.5708 0 1.5708"/>
</visual>
<collision>
<geometry>
<mesh filename="package://mycobot_description/urdf/new_320_pi/link2.dae"/>
</geometry>
<origin xyz = "-0.090 0.0 -0.138 " rpy = " 0 1.5708 0"/>
<origin xyz = "-0.075 -0.115 -0.022 " rpy = " 1.5708 0 1.5708"/>
</collision>
</link>
@ -61,14 +61,14 @@
<!--- 0.0 0 -0.04 -->
<mesh filename="package://mycobot_description/urdf/new_320_pi/link3.dae"/>
</geometry>
<origin xyz = "-0.22 0 0.093 " rpy = " 0 1.5708 0"/>
<origin xyz = "-0.22 -0.115 0.142 " rpy = " 1.5708 0 1.5708"/>
</visual>
<collision>
<geometry>
<!--- 0.0 0 -0.04 -->
<mesh filename="package://mycobot_description/urdf/new_320_pi/link3.dae"/>
</geometry>
<origin xyz = "-0.22 0 0.093 " rpy = " 0 1.5708 0"/>
<origin xyz = "-0.22 -0.115 0.142 " rpy = " 1.5708 0 1.5708"/>
</collision>
</link>
@ -79,14 +79,14 @@
<!--- 0.0 0 -0.04 -->
<mesh filename="package://mycobot_description/urdf/new_320_pi/link4.dae"/>
</geometry>
<origin xyz = "0 0.275 -0.096 " rpy = " 1.5708 1.5708 0"/>
<origin xyz = "-0.115 0.338 -0.1435" rpy = " 1.5708 0 0"/>
</visual>
<collision>
<geometry>
<!--- 0.0 0 -0.04 -->
<mesh filename="package://mycobot_description/urdf/new_320_pi/link4.dae"/>
</geometry>
<origin xyz = "0 0.275 -0.096 " rpy = " 1.5708 1.5708 0"/>
<origin xyz = "-0.115 0.338 -0.1435 " rpy = " 1.5708 0 0"/>
</collision>
</link>
@ -97,14 +97,14 @@
<!--- 0.0 0 -0.04 -->
<mesh filename="package://mycobot_description/urdf/new_320_pi/link5.dae"/>
</geometry>
<origin xyz = "0.0012 -0.044 -0.456 " rpy = " 0 0 -1.5708"/>
<origin xyz = "-0.1149 -0.0322 -0.482 " rpy = " 0 0 0"/>
</visual>
<collision>
<geometry>
<!--- 0.0 0 -0.04 -->
<mesh filename="package://mycobot_description/urdf/new_320_pi/link5.dae"/>
</geometry>
<origin xyz = "0.0012 -0.044 -0.456 " rpy = " 0 0 -1.5708"/>
<origin xyz = "-0.1149 -0.0322 -0.482 " rpy = " 0 0 0"/>
</collision>
</link>
@ -115,30 +115,30 @@
<!--- 0.0 0 -0.04 -->
<mesh filename="package://mycobot_description/urdf/new_320_pi/link6.dae"/>
</geometry>
<origin xyz = "-0.431 0.0009 -0.16536 " rpy = " 0 1.5708 0"/>
<origin xyz = "-0.11478 0.433 -0.15 " rpy = " 1.5708 0 0"/>
</visual>
<collision>
<geometry>
<!--- 0.0 0 -0.04 -->
<mesh filename="package://mycobot_description/urdf/new_320_pi/link6.dae"/>
</geometry>
<origin xyz = "-0.431 0.0009 -0.16536 " rpy = " 0 1.5708 0"/>
<origin xyz = "-0.11478 0.433 -0.15 " rpy = " 1.5708 0 0"/>
</collision>
</link>
<joint name="joint2_to_joint1" type="revolute">
<axis xyz="0 0 1"/>
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
<limit effort = "1000.0" lower = "-2.96" upper = "2.96" velocity = "0"/>
<parent link="base"/>
<child link="link1"/>
<origin xyz= "0 0 0.16846" rpy = "0 0 0"/>
<origin xyz= "0 0 0.162" rpy = "0 0 0"/>
</joint>
<joint name="joint3_to_joint2" type="revolute">
<axis xyz="0 0 1"/>
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
<limit effort = "1000.0" lower = "-2.79" upper = "2.79" velocity = "0"/>
<parent link="link1"/>
<child link="link2"/>
<origin xyz= "0 0 0" rpy = "0 -1.5708 1.5708"/>
@ -147,7 +147,7 @@
<joint name="joint4_to_joint3" type="revolute">
<axis xyz=" 0 0 1"/>
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
<limit effort = "1000.0" lower = "-2.79" upper = "2.79" velocity = "0"/>
<parent link="link2"/>
<child link="link3"/>
<origin xyz= "0.13635 0 0 " rpy = "0 0 0"/>
@ -156,28 +156,29 @@
<joint name="joint5_to_joint4" type="revolute">
<axis xyz=" 0 0 1"/>
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
<limit effort = "1000.0" lower = "-2.79" upper = "2.79" velocity = "0"/>
<parent link="link3"/>
<child link="link4"/>
<origin xyz = "0.12 0 0.09 " rpy = " 0 0 1.57080"/>
<!-- <origin xyz = "0.12 0 0.09 " rpy = " 0 0 1.57080"/> -->
<origin xyz = "0.1205 0 0.082 " rpy = " 0 0 1.57080"/>
</joint>
<joint name="joint6_to_joint5" type="revolute">
<axis xyz="0 0 1"/>
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
<limit effort = "1000.0" lower = "-2.96" upper = "2.96" velocity = "0"/>
<parent link="link4"/>
<child link="link5"/>
<origin xyz= "0 -0.080 0" rpy = "1.5708 0 0"/>
<origin xyz= "0 -0.084 0" rpy = "1.5708 0 0"/>
</joint>
<joint name="joint6output_to_joint6" type="revolute">
<axis xyz="0 0 1"/>
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
<limit effort = "1000.0" lower = "-3.05" upper = "3.05" velocity = "0"/>
<parent link="link5"/>
<child link="link6"/>
<origin xyz= "0 0.0760 0" rpy = "-1.5708 0 0"/>
<origin xyz= "0 0.06635 0" rpy = "-1.5708 0 0"/>
</joint>

42
mycobot_description/urdf/new_320_pi/new_mycobot_pro_320_pi_moveit.urdf
View file

@ -24,10 +24,10 @@
<geometry>
<mesh filename="package://mycobot_description/urdf/new_320_pi/link1.dae"/>
</geometry>
<origin xyz = "-0.29156 -0.220 -0.085 " rpy = " 0 0 1.5708"/>
<origin xyz = "0.115 -0.172 -0.086 " rpy = " 0 0 3.1415926"/>
</visual>
<collision>
<origin xyz = "0 0 -0.02 " rpy = " 0 0 1.5708"/>
<origin xyz = "0 0 -0.02 " rpy = " 0 0 3.14159"/>
<geometry>
<cylinder length="0.106" radius="0.043"/>
</geometry>
@ -40,7 +40,7 @@
<geometry>
<mesh filename="package://mycobot_description/urdf/new_320_pi/link2.dae"/>
</geometry>
<origin xyz = "-0.090 0 -0.138 " rpy = " 0 1.5708 0"/>
<origin xyz = "-0.075 -0.115 -0.022 " rpy = " 1.5708 0 1.5708"/>
</visual>
<collision>
<origin xyz = "0.07 0 0.095 " rpy = " 0 1.5708 0"/>
@ -56,10 +56,10 @@
<geometry>
<mesh filename="package://mycobot_description/urdf/new_320_pi/link3.dae"/>
</geometry>
<origin xyz = "-0.22 0 0.093 " rpy = " 0 1.5708 0"/>
<origin xyz = "-0.22 -0.115 0.142 " rpy = " 1.5708 0 1.5708"/>
</visual>
<collision>
<origin xyz = "0.044 0 0 " rpy = " 0 1.5708 0"/>
<origin xyz = "0.044 0 0 " rpy = " 1.5708 0 1.5708"/>
<geometry>
<cylinder length="0.14" radius="0.0305"/>
</geometry>
@ -73,10 +73,10 @@
<geometry>
<mesh filename="package://mycobot_description/urdf/new_320_pi/link4.dae"/>
</geometry>
<origin xyz = "0 0.275 -0.096 " rpy = " 1.5708 1.5708 0"/>
<origin xyz = "-0.115 0.338 -0.1435" rpy = " 1.5708 0 0"/>
</visual>
<collision>
<origin xyz = "0.0 -0.014 0 " rpy = " 1.5708 1.5708 0"/>
<origin xyz = "0.0 -0.014 0 " rpy = " 1.5708 0 0"/>
<geometry>
<cylinder length="0.083" radius="0.029"/>
</geometry>
@ -89,10 +89,10 @@
<geometry>
<mesh filename="package://mycobot_description/urdf/new_320_pi/link5.dae"/>
</geometry>
<origin xyz = "0.0012 -0.044 -0.456 " rpy = " 0 0 -1.5708"/>
<origin xyz = "-0.1149 -0.0322 -0.482 " rpy = " 0 0 0"/>
</visual>
<collision>
<origin xyz = "0 -0.01 0 " rpy = " 0 0 -1.5708"/>
<origin xyz = "0 -0.01 0 " rpy = " 0 0 0"/>
<geometry>
<cylinder length="0.0849" radius="0.029"/>
</geometry>
@ -107,10 +107,10 @@
<material name = "grey">
<color rgba = "0.5 0.5 0.5 1"/>
</material>
<origin xyz = "-0.431 0.0009 -0.16536 " rpy = " 0 1.5708 0"/>
<origin xyz = "-0.11478 0.433 -0.15 " rpy = " 1.5708 0 0"/>
</visual>
<collision>
<origin xyz = "-0.431 0.0009 -0.16536 " rpy = " 0 1.5708 0"/>
<origin xyz = "-0.431 0.0009 -0.16536 " rpy = " 1.5708 0 0"/>
<geometry>
<cylinder length="0.0225" radius="0.026"/>
</geometry>
@ -119,16 +119,16 @@
<joint name="joint2_to_joint1" type="revolute">
<axis xyz="0 0 1"/>
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
<limit effort = "1000.0" lower = "-2.96" upper = "2.96" velocity = "0"/>
<parent link="base"/>
<child link="link1"/>
<origin xyz= "0 0 0.17846" rpy = "0 0 0"/>
<origin xyz= "0 0 0.162" rpy = "0 0 0"/>
</joint>
<joint name="joint3_to_joint2" type="revolute">
<axis xyz="0 0 1"/>
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
<limit effort = "1000.0" lower = "-2.79" upper = "2.79" velocity = "0"/>
<parent link="link1"/>
<child link="link2"/>
<origin xyz= "0 0 0" rpy = "0 -1.57080 1.57080"/>
@ -137,7 +137,7 @@
<joint name="joint4_to_joint3" type="revolute">
<axis xyz=" 0 0 1"/>
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
<limit effort = "1000.0" lower = "-2.79" upper = "2.79" velocity = "0"/>
<parent link="link2"/>
<child link="link3"/>
<origin xyz= "0.13635 0 0 " rpy = "0 0 0"/>
@ -145,26 +145,26 @@
<joint name="joint5_to_joint4" type="revolute">
<axis xyz=" 0 0 1"/>
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
<limit effort = "1000.0" lower = "-2.79" upper = "2.79" velocity = "0"/>
<parent link="link3"/>
<child link="link4"/>
<origin xyz= "0.12 0 0.090" rpy = "0 0 1.57080"/>
<origin xyz= "0.1205 0 0.082" rpy = "0 0 1.57080"/>
</joint>
<joint name="joint6_to_joint5" type="revolute">
<axis xyz="0 0 1"/>
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
<limit effort = "1000.0" lower = "-2.96" upper = "2.96" velocity = "0"/>
<parent link="link4"/>
<child link="link5"/>
<origin xyz= "0 -0.08515 0" rpy = "1.57080 0 0"/>
<origin xyz= "0 -0.084 0" rpy = "1.57080 0 0"/>
</joint>
<joint name="joint6output_to_joint6" type="revolute">
<axis xyz="0 0 1"/>
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
<limit effort = "1000.0" lower = "-3.05" upper = "3.05" velocity = "0"/>
<parent link="link5"/>
<child link="link6"/>
<origin xyz= "0 0.0760 0" rpy = "-1.57080 0 0 "/>
<origin xyz= "0 0.06635 0" rpy = "-1.57080 0 0 "/>
</joint>
</robot>