6aff781d8f10a0e2a067ccf297fe2cfbb5bb553a to master · CeTI / ROS / ROS Packages / panda_simulation

.devcontainer/Dockerfile

deleted100644 → 0

+0 −6

Original line number	Original line	Diff line number	Diff line
	FROM osrf/ros:melodic-desktop-full-bionic

	RUN apt-get update && apt-get install -q -y \
	openssh-client

	RUN echo 'source /opt/ros/melodic/setup.bash' >> /root/.bashrc
	No newline at end of file

.devcontainer/devcontainer.json

deleted100644 → 0

+0 −21

Original line number	Original line	Diff line number	Diff line
	{
	"name": "panda_simulation-dev",
	"dockerFile": "Dockerfile",
	"extensions": [
	"ms-vscode.cpptools",
	"ms-iot.vscode-ros"
	],
	"runArgs": [
	"--cap-add=SYS_PTRACE",
	"--security-opt",
	"seccomp=unconfined",
	"-v",
	"${env:HOME}${env:USERPROFILE}/.ssh:/root/.ssh"
	],
	"settings": {
	"terminal.integrated.shell.linux": "/bin/bash"
	},
	"postCreateCommand": "bash /catkin_ws/src/panda_simulation/scripts/docker-setup.sh",
	"workspaceMount": "src=/Users/pekel/code/ros/panda_simulation,dst=/catkin_ws/src/panda_simulation,type=bind,consistency=cached",
	"workspaceFolder": "/catkin_ws"
	}
	No newline at end of file

.editorconfig

0 → 100644

+16 −0

Original line number	Original line	Diff line number	Diff line
			root = true

			[*]
			charset = utf-8
			end_of_line = lf
			indent_size = 2
			indent_style = space
			insert_final_newline = false
			max_line_length = 120
			tab_width = 2
			ij_continuation_indent_size = 4
			ij_formatter_off_tag = @formatter:off
			ij_formatter_on_tag = @formatter:on
			ij_formatter_tags_enabled = false
			ij_smart_tabs = false
			ij_wrap_on_typing = false

CMakeLists.txt

+76 −17

Original line number	Original line	Diff line number	Diff line
	@@ -30,15 +30,12 @@ find_package(catkin REQUIRED
	# System dependencies are found with CMake's conventions		# System dependencies are found with CMake's conventions
	find_package(Boost REQUIRED COMPONENTS filesystem)		find_package(Boost REQUIRED COMPONENTS filesystem)

	find_package(PkgConfig REQUIRED)
	pkg_check_modules(JSONCPP jsoncpp)
	message(${JSONCPP_LIBRARIES})

	# ################################################################################################################################		# ################################################################################################################################
	# catkin specific configuration ##		# catkin specific configuration ##
	# ################################################################################################################################		# ################################################################################################################################
	# The catkin_package macro generates cmake config files for your package Declare things to be passed to dependent projects		# The catkin_package macro generates cmake config files for your package Declare things to be passed to dependent projects
	catkin_package(CATKIN_DEPENDS		catkin_package(
			CATKIN_DEPENDS
	moveit_core		moveit_core
	moveit_visual_tools		moveit_visual_tools
	moveit_ros_planning_interface		moveit_ros_planning_interface
	@@ -56,18 +53,80 @@ catkin_package(CATKIN_DEPENDS
	# Specify additional locations of header files Your package locations should be listed before other locations		# Specify additional locations of header files Your package locations should be listed before other locations
	include_directories(${catkin_INCLUDE_DIRS})		include_directories(${catkin_INCLUDE_DIRS})

	add_executable(robot_control_node src/robot_control_node.cpp)		## Declare C++ executables
	add_executable(box_publisher_node src/box_publisher.cpp)		add_executable(${PROJECT_NAME}_robot_state_initializer_node src/robot_state_initializer.cpp)
	add_executable(robot_state_initializer_node src/robot_state_initializer.cpp)
	add_executable(move_group_interface_tutorial src/move_group_interface_tutorial.cpp)

	target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES})
	target_link_libraries(box_publisher_node ${catkin_LIBRARIES})
	target_link_libraries(robot_state_initializer_node ${catkin_LIBRARIES})
	target_link_libraries(move_group_interface_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES})

	# add custom controller as library		## Declare C++ libraries
	add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp)		add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.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


			## 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}_robot_state_initializer_node PROPERTIES OUTPUT_NAME robot_state_initializer_node PREFIX "")


	# Specify libraries to link a library or executable target against		# Specify libraries to link a library or executable target against
	target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})		target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})
			target_link_libraries(${PROJECT_NAME}_robot_state_initializer_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/my_python_script
			# 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_gazebo_zones.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)

README.md

+0 −58

Original line number	Original line	Diff line number	Diff line
	# panda_simulation		# panda_simulation

	![Panda in Gazebo](assets/panda-in-gazebo.png?raw=true "Panda in Gazebo")		![Panda in Gazebo](assets/panda-in-gazebo.png?raw=true "Panda in Gazebo")

	This package was written for ROS melodic running under Ubuntu 18.04. Run the following commands to make sure that all additional packages are installed:

	```
	mkdir -p catkin_ws/src
	cd catkin_ws/src
	git clone https://github.com/erdalpekel/panda_simulation.git
	git clone https://github.com/erdalpekel/panda_moveit_config.git
	git clone --branch simulation https://github.com/erdalpekel/franka_ros.git
	cd ..
	sudo apt-get install libboost-filesystem-dev
	rosdep install --from-paths src --ignore-src -y --skip-keys libfranka
	cd ..
	```
	It is also important that you build the libfranka library from source and pass its directory to catkin_make when building this ROS package as described in [this tutorial](https://frankaemika.github.io/docs/installation.html#building-from-source).

	Currently it includes a controller parameter config file and a launch file to launch the [Gazebo](http://gazebosim.org) simulation environment and the Panda robot from FRANKA EMIKA in it with the necessary controllers.

	Build the catkin workspace and run the simulation:
	```
	catkin_make -j4 -DCMAKE_BUILD_TYPE=Release -DFranka_DIR:PATH=/path/to/libfranka/build
	source devel/setup.bash
	roslaunch panda_simulation simulation.launch
	```

	Depending on your operating systems language you might need to export the numeric type so that rviz can read the floating point numbers in the robot model correctly:

	```
	export LC_NUMERIC="en_US.UTF-8"
	```
	Otherwise, the robot will appear in rviz in a collapsed state.


	You can see the full explanation in my [blog post](https://erdalpekel.de/?p=55).

	## Changelog:

	[_MoveIt!_ constraint-aware planning](https://erdalpekel.de/?p=123)

	This repository was extended with a ROS node that communicates with the _MoveIt!_ Planning Scene API. It makes sure that the motion planning pipeline avoids collision objects in the environment specified by the user in a separate directory (`~/.panda_simulation`) as _json_ files.

	[Publishing a box at Panda's hand in _Gazebo_](https://erdalpekel.de/?p=123)

	This repository was extended with a node that publishes a simple box object in the _Gazebo_ simulation at the hand of the robot. The position of this box will get updated as soon as the robot moves.

	[Visual Studio Code Remote Docker](https://erdalpekel.de/?p=123)

	I have added configuration files and additional setup scripts for developing and using this ROS package within a Docker container. Currently user interfaces for Gazebo and RViz are not supported.

	[Position based trajectory execution](https://erdalpekel.de/?p=285)

	The joint specifications in Gazebo were changed from an effort interface to position based interface. Furthermore, the PID controller was substituted with the simple gazebo internal position based control mechanism for a more stable movement profile of the robot. A custom joint position based controller was implemented in order to set the initial joint states of the robot to a valid configuration.

	[Automatic robot state initialization](https://erdalpekel.de/?p=314)

	A separate ROS node was implemented that starts a custom joint position controller and initializes the robot with a specific configuration. It switches back to the default controllers after the robot reaches the desired state.

	![Panda state initialization in Gazebo](assets/robot-state-initializer.gif?raw=true "Panda state initialization in Gazebo")
	No newline at end of file

assets/robot-state-initializer.gif

deleted100644 → 0

−117 KiB

117.40 KiB

config/panda_control.yaml

+19 −3

Original line number	Original line	Diff line number	Diff line
	@@ -3,8 +3,9 @@
	type: joint_state_controller/JointStateController		type: joint_state_controller/JointStateController
	publish_rate: 100		publish_rate: 100

	panda_arm_controller:		position_joint_trajectory_controller:
	type: position_controllers/JointTrajectoryController		type: position_controllers/JointTrajectoryController
			# type: effort_controllers/JointTrajectoryController
	joints:		joints:
	- panda_joint1		- panda_joint1
	- panda_joint2		- panda_joint2
	@@ -17,15 +18,30 @@
	constraints:		constraints:
	goal_time: 2.0		goal_time: 2.0

			gains:
			panda_joint1: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 }
			panda_joint2: { p: 30000, d: 100, i: 0.02, i_clamp: 10000 }
			panda_joint3: { p: 18000, d: 50, i: 0.01, i_clamp: 1 }
			panda_joint4: { p: 18000, d: 70, i: 0.01, i_clamp: 10000 }
			panda_joint5: { p: 12000, d: 70, i: 0.01, i_clamp: 1 }
			panda_joint6: { p: 7000, d: 50, i: 0.01, i_clamp: 1 }
			panda_joint7: { p: 2000, d: 20, i: 0.0, i_clamp: 1 }

	state_publish_rate: 100		state_publish_rate: 100

	panda_hand_controller:		panda_hand_controller:
	type: position_controllers/JointTrajectoryController		# type: position_controllers/JointTrajectoryController
			type: effort_controllers/JointTrajectoryController
	joints:		joints:
	- panda_finger_joint1		- panda_finger_joint1
	- panda_finger_joint2		- panda_finger_joint2

			gains:
			panda_finger_joint1: { p: 5, d: 3.0, i: 0, i_clamp: 1 }
			panda_finger_joint2: { p: 5, d: 1.0, i: 0, i_clamp: 1 }

	state_publish_rate: 25		state_publish_rate: 25

	joint_position_controller:		joint_position_controller:
	type: panda_simulation/JointPositionController		type: panda_simulation/JointPositionController
	arm_id: panda		arm_id: panda

launch/box.launch

deleted100644 → 0

+0 −9

Original line number	Original line	Diff line number	Diff line
	<launch>
	<param name="box_description" command="$(find xacro)/xacro $(find panda_simulation)/models/box.xacro"/>
	<node name="box_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" args="robot_description:=box_description" />
	<node name="spawn_object" pkg="gazebo_ros" type="spawn_model" args="-param box_description -urdf -model box"/>


	<!-- launch node -->
	<node pkg="panda_simulation" type="box_publisher_node" name="box_publisher_node" />
	</launch>
	No newline at end of file

launch/move_group_interface_tutorial.launch

deleted100644 → 0

+0 −66

Original line number	Original line	Diff line number	Diff line
	<launch>

	<param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />

	<!-- GAZEBO arguments -->
	<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" />
	<arg name="load_gripper" default="true" />

	<!--launch GAZEBO with own world configuration -->
	<include file="$(find gazebo_ros)/launch/empty_world.launch">
	<!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.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="rqt_console" pkg="rqt_console" type="rqt_console" />
	<!-- <node name="rqt_logger_level" pkg="rqt_logger_level" type="rqt_logger_level" /> -->

	<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda" />

	<!-- Load joint controller configurations from YAML file to parameter server -->
	<rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />


	<!-- load the controllers -->
	<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" />
	<node if="$(arg load_gripper)" name="controller_spawner_hand" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda_hand_controller" />


	<!-- convert joint states to TF transforms for rviz, etc -->
	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />

	<include file="$(find panda_moveit_config)/launch/planning_context.launch">
	<arg name="load_robot_description" value="true" />
	<arg name="load_gripper" value="$(arg load_gripper)" />
	</include>
	<include file="$(find panda_moveit_config)/launch/move_group.launch">
	<arg name="load_gripper" value="$(arg load_gripper)" />
	</include>
	<group if="$(arg gui)">
	<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
	</group>

	<node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />

	<!-- launch robot control node for moveit motion planning -->
	<node pkg="panda_simulation" type="robot_control_node" name="robot_control_node" output="screen" />

	<!-- load (not start!) custom joint position controller -->
	<node pkg="controller_manager" type="spawner" name="joint_position_launcher" args="--stopped joint_position_controller" />

	<!-- run custom node for automatic intialization -->
	<node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />

	<node pkg="panda_simulation" type="move_group_interface_tutorial" name="move_group_interface_tutorial" respawn="false" output="screen">

	</node>

	</launch>

launch/simulation.launch

+7 −11

Original line number	Original line	Diff line number	Diff line
	<launch>		<launch>
	<param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />

	<!-- GAZEBO arguments -->		<!-- GAZEBO arguments -->
	<arg name="paused" default="false" />		<arg name="paused" default="false" />
	@@ -8,10 +7,16 @@
	<arg name="headless" default="false" />		<arg name="headless" default="false" />
	<arg name="debug" default="false" />		<arg name="debug" default="false" />
	<arg name="load_gripper" default="true" />		<arg name="load_gripper" default="true" />
			<arg name="slow_simulation" default="false" />

			<!-- don't include franka_control, but include the controller stuff from there -->
			<param name="robot_description" command="$(find xacro)/xacro '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'" if="$(arg load_gripper)" />
			<param name="robot_description" command="$(find xacro)/xacro '$(find franka_description)/robots/panda_arm.urdf.xacro'" unless="$(arg load_gripper)" />

	<!--launch GAZEBO with own world configuration -->		<!--launch GAZEBO with own world configuration -->
	<include file="$(find gazebo_ros)/launch/empty_world.launch">		<include file="$(find gazebo_ros)/launch/empty_world.launch">
	<!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.world"/> -->		<!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.world"/> -->
			<arg name="world_name" if="$(arg slow_simulation)" value="$(find panda_simulation)/worlds/simulator.world"/>
	<arg name="debug" value="$(arg debug)" />		<arg name="debug" value="$(arg debug)" />
	<arg name="gui" value="$(arg gui)" />		<arg name="gui" value="$(arg gui)" />
	<arg name="paused" value="$(arg paused)" />		<arg name="paused" value="$(arg paused)" />
	@@ -19,17 +24,14 @@
	<arg name="headless" value="$(arg headless)" />		<arg name="headless" value="$(arg headless)" />
	</include>		</include>

	<node name="rqt_console" pkg="rqt_console" type="rqt_console" />
	<!-- <node name="rqt_logger_level" pkg="rqt_logger_level" type="rqt_logger_level" /> -->

	<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda" />		<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda" />

	<!-- Load joint controller configurations from YAML file to parameter server -->		<!-- Load joint controller configurations from YAML file to parameter server -->
	<rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />		<rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />


	<!-- load the controllers -->		<!-- load the controllers -->
	<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" />		<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller position_joint_trajectory_controller" />
	<node if="$(arg load_gripper)" name="controller_spawner_hand" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda_hand_controller" />		<node if="$(arg load_gripper)" name="controller_spawner_hand" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda_hand_controller" />


	@@ -43,15 +45,9 @@
	<include file="$(find panda_moveit_config)/launch/move_group.launch">		<include file="$(find panda_moveit_config)/launch/move_group.launch">
	<arg name="load_gripper" value="$(arg load_gripper)" />		<arg name="load_gripper" value="$(arg load_gripper)" />
	</include>		</include>
	<group if="$(arg gui)">
	<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
	</group>

	<node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />		<node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />

	<!-- launch robot control node for moveit motion planning -->
	<node pkg="panda_simulation" type="robot_control_node" name="robot_control_node" output="screen" />

	<!-- load (not start!) custom joint position controller -->		<!-- load (not start!) custom joint position controller -->
	<node pkg="controller_manager" type="spawner" name="joint_position_launcher" args="--stopped joint_position_controller" />		<node pkg="controller_manager" type="spawner" name="joint_position_launcher" args="--stopped joint_position_controller" />

mainpage.dox

0 → 100644

+11 −0

Original line number	Original line	Diff line number	Diff line
			/**
			\mainpage

			\htmlinclude manifest.html

			<!--
			Provide an overview of your package.
			-->


			*/

models/box.xacro

deleted100644 → 0

+0 −40

Original line number	Original line	Diff line number	Diff line
	<?xml version="1.0"?>
	<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="simple_box">
	<link name="object_base_link">
	</link>

	<joint name="object_base_joint" type="fixed">
	<parent link="object_base_link"/>
	<child link="object_link"/>
	<axis xyz="0 0 1" />
	<origin xyz="0 0 0" rpy="0 0 0"/>
	</joint>

	<link name="object_link">
	<inertial>
	<origin xyz="0 0 0" />
	<mass value="1.0" />
	<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="100.0" iyz="0.0" izz="1.0" />
	</inertial>
	<visual>
	<origin xyz="0 0 0"/>
	<geometry>
	<box size="0.04 0.04 0.08" />
	</geometry>
	</visual>
	<collision>
	<origin xyz="0 0 0"/>
	<geometry>
	<box size="0.04 0.04 0.08" />
	</geometry>
	</collision>
	</link>

	<gazebo reference="object_base_link">
	<gravity>0</gravity>
	</gazebo>

	<gazebo reference="object_link">
	<material>Gazebo/Blue</material>
	</gazebo>
	</robot>
	No newline at end of file

package.xml

+26 −5

Original line number	Original line	Diff line number	Diff line
	<?xml version="1.0"?>		<?xml version="1.0"?>
	<package format="2">		<package format="2">
	<name>panda_simulation</name>		<name>panda_simulation</name>
	<version>0.0.0</version>		<version>0.2.0</version>
	<description>The panda_simulation package</description>		<description>The panda_simulation package</description>

	<!-- One maintainer tag required, multiple allowed, one person per tag -->		<!-- One maintainer tag required, multiple allowed, one person per tag -->
	<!-- Example: -->		<!-- Example: -->
	<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->		<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
	<maintainer email="erdal@todo.todo">erdal</maintainer>		<maintainer email="johannes.mey@tu-dresden.de">Johannes Mey</maintainer>
			<maintainer email="sebastian.ebert@tu-dresden.de">Sebastian Ebert</maintainer>

			<!-- 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/gazebo_zones</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> -->
			<author email="johannes.mey@tu-dresden.de">Johannes Mey</author>
			<author email="sebastian.ebert@tu-dresden.de">Sebastian Ebert</author>
			<author>Erdal Pekel</author>

	<!-- One license tag required, multiple allowed, one license per tag -->		<!-- One license tag required, multiple allowed, one license per tag -->
	<!-- Commonly used license strings: -->		<!-- Commonly used license strings: -->
	<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->		<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
	<license>TODO</license>		<license>MIT</license>


	<buildtool_depend>catkin</buildtool_depend>		<buildtool_depend>catkin</buildtool_depend>

	<depend>controller_manager</depend>		<depend>controller_manager</depend>
	<depend>effort_controllers</depend>		<depend>effort_controllers</depend>
	<depend>gazebo_ros</depend>		<!-- <depend>gazebo_ros</depend>-->
			<depend>gazebo_plugins</depend>
	<depend>gazebo_ros_control</depend>		<depend>gazebo_ros_control</depend>
	<depend>joint_state_controller</depend>		<depend>joint_state_controller</depend>
	<depend>joint_state_publisher</depend>		<depend>joint_state_publisher</depend>
	@@ -30,7 +45,6 @@
	<depend>std_msgs</depend>		<depend>std_msgs</depend>
	<depend>tf</depend>		<depend>tf</depend>
	<depend>xacro</depend>		<depend>xacro</depend>
	<depend>libjsoncpp-dev</depend>
	<depend>moveit_simple_controller_manager</depend>		<depend>moveit_simple_controller_manager</depend>

	<!-- CUSTOM -->		<!-- CUSTOM -->
	@@ -41,10 +55,17 @@
	<depend>controller_interface</depend>		<depend>controller_interface</depend>
	<depend>hardware_interface</depend>		<depend>hardware_interface</depend>


			<build_depend>sdformat</build_depend>
			<exec_depend>sdformat</exec_depend>

			<exec_depend>gazebo_ros</exec_depend>

	<exec_depend>pluginlib</exec_depend>		<exec_depend>pluginlib</exec_depend>

	<!-- The export tag contains other, unspecified, tags -->		<!-- The export tag contains other, unspecified, tags -->
	<export>		<export>
	<controller_interface plugin="${prefix}/controller_plugins.xml" />		<controller_interface plugin="${prefix}/controller_plugins.xml" />
			<gazebo_ros gazebo_media_path="/usr/share/gazebo-9/media/materials/scripts"/>
	</export>		</export>
	</package>		</package>

rosdoc.yaml

0 → 100644

+4 −0

Original line number	Original line	Diff line number	Diff line
			- builder: doxygen
			name: C++ API
			file_patterns: '.c .cpp .h .cc .hh .dox'
			homepage: https://st.inf.tu-dresden.de/ceti-robots

scripts/docker-setup.sh

deleted100644 → 0

+0 −33

Original line number	Original line	Diff line number	Diff line
	#!/bin/bash

	REPOSITORY_DIR=$(pwd)
	WORKSPACE_DIR=/catkin_ws
	DEPENDENCIES_DIR=$WORKSPACE_DIR/dependencies
	APP_DIR=/root/.panda_simulation

	# Install libfranka
	mkdir $DEPENDENCIES_DIR && cd $DEPENDENCIES_DIR
	git clone --recursive https://github.com/frankaemika/libfranka
	cd libfranka
	git checkout 0.5.0
	git submodule update
	mkdir build && cd build
	cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=0 ..
	cmake --build . -- -j$(nproc)

	# Clone the franka_ros and panda_moveit_config repositories for simulating Panda robot
	cd $WORKSPACE_DIR/src
	git clone https://github.com/erdalpekel/panda_moveit_config.git
	git clone --branch simulation https://github.com/erdalpekel/franka_ros.git

	# Install package dependencies with rosdep
	cd $WORKSPACE_DIR
	rosdep install --from-paths src --ignore-src -y --skip-keys libfranka --skip-keys moveit_perception

	# Clone the Visual-Studio-Code-ROS repository into the workspace directory /catkin_ws
	cd $WORKSPACE_DIR
	git clone git@github.com:erdalpekel/Visual-Studio-Code-ROS.git
	mv Visual-Studio-Code-ROS .vscode

	# create app directory for config files
	mkdir $APP_DIR
	No newline at end of file

src/box_publisher.cpp

deleted100644 → 0

+0 −71

Original line number	Original line	Diff line number	Diff line
	#include <gazebo_msgs/SetModelState.h>
	#include <moveit/move_group_interface/move_group_interface.h>
	#include <moveit/robot_model_loader/robot_model_loader.h>
	#include <ros/ros.h>
	#include <sensor_msgs/JointState.h>

	static const std::string PLANNING_GROUP_ARM = "panda_arm";
	static const double PANDA_ARM_TO_HAND_OFFSET = 0.12;
	static const double PANDA_HAND_TO_FINGER_OFFSET = 0.04;
	ros::Publisher gazebo_model_state_pub;
	robot_model::RobotModelPtr kinematic_model;
	robot_state::RobotStatePtr kinematic_state;

	void jointStatesCallback(const sensor_msgs::JointState &joint_states_current)
	{
	const robot_state::JointModelGroup *joint_model_group = kinematic_model->getJointModelGroup("panda_arm");
	const std::vector<std::string> &joint_names = joint_model_group->getJointModelNames();

	std::vector<double> joint_states;
	for (size_t i = 0; i < joint_states_current.position.size() - 2; ++i)
	{
	joint_states.push_back(joint_states_current.position[i + 2]);
	}
	kinematic_state->setJointGroupPositions(joint_model_group, joint_states);

	const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("panda_link8");

	double end_effector_z_offset = PANDA_ARM_TO_HAND_OFFSET + PANDA_HAND_TO_FINGER_OFFSET;
	Eigen::Affine3d tmp_transform(Eigen::Translation3d(Eigen::Vector3d(0.0, 0.0, end_effector_z_offset)));

	Eigen::Affine3d newState = end_effector_state * tmp_transform;

	geometry_msgs::Pose pose;
	pose.position.x = newState.translation().x();
	pose.position.y = newState.translation().y();
	pose.position.z = newState.translation().z();

	Eigen::Quaterniond quat(newState.rotation());
	pose.orientation.w = quat.w();
	pose.orientation.x = quat.x();
	pose.orientation.y = quat.y();
	pose.orientation.z = quat.z();

	ROS_DEBUG_STREAM("translation" << std::endl << newState.translation());
	ROS_DEBUG_STREAM("rotation" << std::endl << newState.rotation());

	gazebo_msgs::ModelState model_state;
	// This string results from the spawn_urdf call in the box.launch file argument: -model box
	model_state.model_name = std::string("box");
	model_state.pose = pose;
	model_state.reference_frame = std::string("world");

	gazebo_model_state_pub.publish(model_state);
	}

	int main(int argc, char **argv)
	{
	ros::init(argc, argv, "box_publisher_node");
	ros::NodeHandle node_handle;

	ros::Subscriber joint_states_sub = node_handle.subscribe("/joint_states", 1, jointStatesCallback);

	robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
	kinematic_model = robot_model_loader.getModel();
	kinematic_state = robot_state::RobotStatePtr(new robot_state::RobotState(kinematic_model));

	gazebo_model_state_pub = node_handle.advertise<gazebo_msgs::ModelState>("/gazebo/set_model_state", 1);

	ros::spin();
	return 0;
	}
	No newline at end of file

src/move_group_interface_tutorial.cpp

deleted100644 → 0

+0 −406

Original line number	Original line	Diff line number	Diff line
	/*********************************************************************
	* Software License Agreement (BSD License)
	*
	* Copyright (c) 2013, SRI International
	* All rights reserved.
	*
	* Redistribution and use in source and binary forms, with or without
	* modification, are permitted provided that the following conditions
	* are met:
	*
	* * Redistributions of source code must retain the above copyright
	* notice, this list of conditions and the following disclaimer.
	* * 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.
	* * Neither the name of SRI International nor the names of its
	* contributors may be used to endorse or promote products derived
	* from this software without specific prior written permission.
	*
	* 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 OWNER 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.
	*********************************************************************/

	/* Author: Sachin Chitta, Dave Coleman, Mike Lautman */

	#include <moveit/move_group_interface/move_group_interface.h>
	#include <moveit/planning_scene_interface/planning_scene_interface.h>

	#include <moveit_msgs/DisplayRobotState.h>
	#include <moveit_msgs/DisplayTrajectory.h>

	#include <moveit_msgs/AttachedCollisionObject.h>
	#include <moveit_msgs/CollisionObject.h>

	#include <moveit_visual_tools/moveit_visual_tools.h>

	int main(int argc, char** argv)
	{
	ros::init(argc, argv, "move_group_interface_tutorial");
	ros::NodeHandle node_handle;
	ros::AsyncSpinner spinner(1);
	spinner.start();

	// BEGIN_TUTORIAL
	//
	// Setup
	// ^^^^^
	//
	// MoveIt operates on sets of joints called "planning groups" and stores them in an object called
	// the `JointModelGroup`. Throughout MoveIt the terms "planning group" and "joint model group"
	// are used interchangably.
	static const std::string PLANNING_GROUP = "panda_arm";

	// The :planning_interface:`MoveGroupInterface` class can be easily
	// setup using just the name of the planning group you would like to control and plan for.
	moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

	// We will use the :planning_interface:`PlanningSceneInterface`
	// class to add and remove collision objects in our "virtual world" scene
	moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

	// Raw pointers are frequently used to refer to the planning group for improved performance.
	const robot_state::JointModelGroup* joint_model_group =
	move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

	// Visualization
	// ^^^^^^^^^^^^^
	//
	// The package MoveItVisualTools provides many capabilities for visualizing objects, robots,
	// and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script.
	namespace rvt = rviz_visual_tools;
	moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
	visual_tools.deleteAllMarkers();

	// Remote control is an introspection tool that allows users to step through a high level script
	// via buttons and keyboard shortcuts in RViz
	visual_tools.loadRemoteControl();

	// RViz provides many types of markers, in this demo we will use text, cylinders, and spheres
	Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
	text_pose.translation().z() = 1.75;
	visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);

	// Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
	visual_tools.trigger();

	// Getting Basic Information
	// ^^^^^^^^^^^^^^^^^^^^^^^^^
	//
	// We can print the name of the reference frame for this robot.
	ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str());

	// We can also print the name of the end-effector link for this group.
	ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());

	// We can get a list of all the groups in the robot:
	ROS_INFO_NAMED("tutorial", "Available Planning Groups:");
	std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
	std::ostream_iterator<std::string>(std::cout, ", "));

	// Start the demo
	// ^^^^^^^^^^^^^^^^^^^^^^^^^
	visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

	// Planning to a Pose goal
	// ^^^^^^^^^^^^^^^^^^^^^^^
	// We can plan a motion for this group to a desired pose for the
	// end-effector.
	geometry_msgs::Pose target_pose1;
	target_pose1.orientation.w = 1.0;
	target_pose1.position.x = 0.28;
	target_pose1.position.y = -0.2;
	target_pose1.position.z = 0.5;
	move_group.setPoseTarget(target_pose1);

	// Now, we call the planner to compute the plan and visualize it.
	// Note that we are just planning, not asking move_group
	// to actually move the robot.
	moveit::planning_interface::MoveGroupInterface::Plan my_plan;

	bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

	ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");

	// Visualizing plans
	// ^^^^^^^^^^^^^^^^^
	// We can also visualize the plan as a line with markers in RViz.
	ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line");
	visual_tools.publishAxisLabeled(target_pose1, "pose1");
	visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);
	visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
	visual_tools.trigger();
	visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

	// Moving to a pose goal
	// ^^^^^^^^^^^^^^^^^^^^^
	//
	// Moving to a pose goal is similar to the step above
	// except we now use the move() function. Note that
	// the pose goal we had set earlier is still active
	// and so the robot will try to move to that goal. We will
	// not use that function in this tutorial since it is
	// a blocking function and requires a controller to be active
	// and report success on execution of a trajectory.

	/* Uncomment below line when working with a real robot */
	move_group.move();

	// Planning to a joint-space goal
	// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
	//
	// Let's set a joint space goal and move towards it. This will replace the
	// pose target we set above.
	//
	// To start, we'll create an pointer that references the current robot's state.
	// RobotState is the object that contains all the current position/velocity/acceleration data.
	moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
	//
	// Next get the current set of joint values for the group.
	std::vector<double> joint_group_positions;
	current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);

	// Now, let's modify one of the joints, plan to the new joint space goal and visualize the plan.
	joint_group_positions[0] = -1.0; // radians
	move_group.setJointValueTarget(joint_group_positions);

	// We lower the allowed maximum velocity and acceleration to 5% of their maximum.
	// The default values are 10% (0.1).
	// Set your preferred defaults in the joint_limits.yaml file of your robot's moveit_config
	// or set explicit factors in your code if you need your robot to move faster.
	move_group.setMaxVelocityScalingFactor(0.05);
	move_group.setMaxAccelerationScalingFactor(0.05);

	success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
	ROS_INFO_NAMED("tutorial", "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");

	// Visualize the plan in RViz
	visual_tools.deleteAllMarkers();
	visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
	visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
	visual_tools.trigger();
	visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

	// Planning with Path Constraints
	// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
	//
	// Path constraints can easily be specified for a link on the robot.
	// Let's specify a path constraint and a pose goal for our group.
	// First define the path constraint.
	moveit_msgs::OrientationConstraint ocm;
	ocm.link_name = "panda_link7";
	ocm.header.frame_id = "panda_link0";
	ocm.orientation.w = 1.0;
	ocm.absolute_x_axis_tolerance = 0.1;
	ocm.absolute_y_axis_tolerance = 0.1;
	ocm.absolute_z_axis_tolerance = 0.1;
	ocm.weight = 1.0;

	// Now, set it as the path constraint for the group.
	moveit_msgs::Constraints test_constraints;
	test_constraints.orientation_constraints.push_back(ocm);
	move_group.setPathConstraints(test_constraints);

	// We will reuse the old goal that we had and plan to it.
	// Note that this will only work if the current state already
	// satisfies the path constraints. So we need to set the start
	// state to a new pose.
	robot_state::RobotState start_state(*move_group.getCurrentState());
	geometry_msgs::Pose start_pose2;
	start_pose2.orientation.w = 1.0;
	start_pose2.position.x = 0.55;
	start_pose2.position.y = -0.05;
	start_pose2.position.z = 0.8;
	start_state.setFromIK(joint_model_group, start_pose2);
	move_group.setStartState(start_state);

	// Now we will plan to the earlier pose target from the new
	// start state that we have just created.
	move_group.setPoseTarget(target_pose1);

	// Planning with constraints can be slow because every sample must call an inverse kinematics solver.
	// Lets increase the planning time from the default 5 seconds to be sure the planner has enough time to succeed.
	move_group.setPlanningTime(10.0);

	success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
	ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");

	// Visualize the plan in RViz
	visual_tools.deleteAllMarkers();
	visual_tools.publishAxisLabeled(start_pose2, "start");
	visual_tools.publishAxisLabeled(target_pose1, "goal");
	visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE);
	visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
	visual_tools.trigger();
	visual_tools.prompt("next step");

	// When done with the path constraint be sure to clear it.
	move_group.clearPathConstraints();

	// Cartesian Paths
	// ^^^^^^^^^^^^^^^
	// You can plan a Cartesian path directly by specifying a list of waypoints
	// for the end-effector to go through. Note that we are starting
	// from the new start state above. The initial pose (start state) does not
	// need to be added to the waypoint list but adding it can help with visualizations
	std::vector<geometry_msgs::Pose> waypoints;
	waypoints.push_back(start_pose2);

	geometry_msgs::Pose target_pose3 = start_pose2;

	target_pose3.position.z -= 0.2;
	waypoints.push_back(target_pose3); // down

	target_pose3.position.y -= 0.2;
	waypoints.push_back(target_pose3); // right

	target_pose3.position.z += 0.2;
	target_pose3.position.y += 0.2;
	target_pose3.position.x -= 0.2;
	waypoints.push_back(target_pose3); // up and left

	// We want the Cartesian path to be interpolated at a resolution of 1 cm
	// which is why we will specify 0.01 as the max step in Cartesian
	// translation. We will specify the jump threshold as 0.0, effectively disabling it.
	// Warning - disabling the jump threshold while operating real hardware can cause
	// large unpredictable motions of redundant joints and could be a safety issue
	moveit_msgs::RobotTrajectory trajectory;
	const double jump_threshold = 0.0;
	const double eef_step = 0.01;
	double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
	ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0);

	// Visualize the plan in RViz
	visual_tools.deleteAllMarkers();
	visual_tools.publishText(text_pose, "Cartesian Path", rvt::WHITE, rvt::XLARGE);
	visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
	for (std::size_t i = 0; i < waypoints.size(); ++i)
	visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
	visual_tools.trigger();
	visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

	// Cartesian motions should often be slow, e.g. when approaching objects. The speed of cartesian
	// plans cannot currently be set through the maxVelocityScalingFactor, but requires you to time
	// the trajectory manually, as described [here](https://groups.google.com/forum/#!topic/moveit-users/MOoFxy2exT4).
	// Pull requests are welcome.

	// You can execute a trajectory by wrapping it in a plan like this.
	moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
	cartesian_plan.trajectory_ = trajectory;
	move_group.execute(cartesian_plan);

	// Adding/Removing Objects and Attaching/Detaching Objects
	// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
	//
	// Define a collision object ROS message.
	moveit_msgs::CollisionObject collision_object;
	collision_object.header.frame_id = move_group.getPlanningFrame();

	// The id of the object is used to identify it.
	collision_object.id = "box1";

	// Define a box to add to the world.
	shape_msgs::SolidPrimitive primitive;
	primitive.type = primitive.BOX;
	primitive.dimensions.resize(3);
	primitive.dimensions[0] = 0.4;
	primitive.dimensions[1] = 0.1;
	primitive.dimensions[2] = 0.4;

	// Define a pose for the box (specified relative to frame_id)
	geometry_msgs::Pose box_pose;
	box_pose.orientation.w = 1.0;
	box_pose.position.x = 0.4;
	box_pose.position.y = -0.2;
	box_pose.position.z = 1.0;

	collision_object.primitives.push_back(primitive);
	collision_object.primitive_poses.push_back(box_pose);
	collision_object.operation = collision_object.ADD;

	std::vector<moveit_msgs::CollisionObject> collision_objects;
	collision_objects.push_back(collision_object);

	// Now, let's add the collision object into the world
	ROS_INFO_NAMED("tutorial", "Add an object into the world");
	planning_scene_interface.addCollisionObjects(collision_objects);

	// Show text in RViz of status
	visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
	visual_tools.trigger();

	// Wait for MoveGroup to receive and process the collision object message
	visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");

	// Now when we plan a trajectory it will avoid the obstacle
	move_group.setStartState(*move_group.getCurrentState());
	geometry_msgs::Pose another_pose;
	another_pose.orientation.w = 1.0;
	another_pose.position.x = 0.4;
	another_pose.position.y = -0.4;
	another_pose.position.z = 0.9;
	move_group.setPoseTarget(another_pose);

	success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
	ROS_INFO_NAMED("tutorial", "Visualizing plan 5 (pose goal move around cuboid) %s", success ? "" : "FAILED");

	// Visualize the plan in RViz
	visual_tools.deleteAllMarkers();
	visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE);
	visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
	visual_tools.trigger();
	visual_tools.prompt("next step");

	// Now, let's attach the collision object to the robot.
	ROS_INFO_NAMED("tutorial", "Attach the object to the robot");
	move_group.attachObject(collision_object.id);

	// Show text in RViz of status
	visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE);
	visual_tools.trigger();

	/* Wait for MoveGroup to receive and process the attached collision object message */
	visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object attaches to the "
	"robot");

	// Now, let's detach the collision object from the robot.
	ROS_INFO_NAMED("tutorial", "Detach the object from the robot");
	move_group.detachObject(collision_object.id);

	// Show text in RViz of status
	visual_tools.publishText(text_pose, "Object dettached from robot", rvt::WHITE, rvt::XLARGE);
	visual_tools.trigger();

	/* Wait for MoveGroup to receive and process the attached collision object message */
	visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object detaches to the "
	"robot");

	// Now, let's remove the collision object from the world.
	ROS_INFO_NAMED("tutorial", "Remove the object from the world");
	std::vector<std::string> object_ids;
	object_ids.push_back(collision_object.id);
	planning_scene_interface.removeCollisionObjects(object_ids);

	// Show text in RViz of status
	visual_tools.publishText(text_pose, "Object removed", rvt::WHITE, rvt::XLARGE);
	visual_tools.trigger();

	/* Wait for MoveGroup to receive and process the attached collision object message */
	visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears");

	// END_TUTORIAL

	ros::shutdown();
	return 0;
	}

src/robot_control_node.cpp

deleted100644 → 0

+0 −140

Original line number	Original line	Diff line number	Diff line
	#include <jsoncpp/json/json.h>
	#include <moveit/move_group_interface/move_group_interface.h>
	#include <moveit/planning_scene/planning_scene.h>
	#include <moveit/planning_scene_interface/planning_scene_interface.h>
	#include <moveit_visual_tools/moveit_visual_tools.h>
	#include <ros/ros.h>
	#include <boost/filesystem.hpp>

	static const std::string PLANNING_GROUP_ARM = "panda_arm";
	static const std::string APP_DIRECTORY_NAME = ".panda_simulation";

	moveit_msgs::CollisionObject extractObstacleFromJson(Json::Value &root, std::string name)
	{
	moveit_msgs::CollisionObject collision_object;
	collision_object.header.frame_id = "world";
	collision_object.id = name;

	const Json::Value dimensions = root["dimensions"];
	ROS_INFO_STREAM("Extracted dimensions: " << dimensions);
	// Define a box to add to the world.
	shape_msgs::SolidPrimitive primitive;
	primitive.type = primitive.BOX;
	primitive.dimensions.resize(3);
	primitive.dimensions[0] = dimensions["x"].asDouble();
	primitive.dimensions[1] = dimensions["y"].asDouble();
	primitive.dimensions[2] = dimensions["z"].asDouble();

	const Json::Value position = root["position"];
	ROS_INFO_STREAM("Extracted position: " << position);

	const Json::Value orientation = root["orientation"];
	ROS_INFO_STREAM("Extracted orientation: " << orientation);
	// Define a pose for the box (specified relative to frame_id)
	geometry_msgs::Pose box_pose;
	box_pose.orientation.w = orientation["w"].asDouble();
	box_pose.orientation.x = orientation["x"].asDouble();
	box_pose.orientation.y = orientation["y"].asDouble();
	box_pose.orientation.z = orientation["z"].asDouble();

	// MoveIt! planning scene expects the center of the object as position.
	// We add half of its dimension to its position
	box_pose.position.x = position["x"].asDouble() + primitive.dimensions[0] / 2.0;
	box_pose.position.y = position["y"].asDouble() + primitive.dimensions[1] / 2.0;
	box_pose.position.z = position["z"].asDouble() + primitive.dimensions[2] / 2.0;

	collision_object.primitives.push_back(primitive);
	collision_object.primitive_poses.push_back(box_pose);
	collision_object.operation = collision_object.ADD;

	return std::move(collision_object);
	}

	int main(int argc, char **argv)
	{
	namespace fs = boost::filesystem;
	ROS_INFO("RUNNING robot_control_node");

	ros::init(argc, argv, "robot_control_node");

	ros::NodeHandle node_handle;
	ros::AsyncSpinner spinner(1);
	spinner.start();

	moveit::planning_interface::MoveGroupInterface move_group_arm(PLANNING_GROUP_ARM);

	ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
	ros::WallDuration sleep_t(0.5);
	while (planning_scene_diff_publisher.getNumSubscribers() < 1)
	{
	sleep_t.sleep();
	}
	moveit_msgs::PlanningScene planning_scene;

	// read JSON files from ~/.panda_simulation
	fs::path home(getenv("HOME"));
	if (fs::is_directory(home))
	{
	fs::path app_directory(home);
	app_directory /= APP_DIRECTORY_NAME;

	if (!fs::exists(app_directory) && !fs::is_directory(app_directory))
	{
	ROS_WARN_STREAM(app_directory << " does not exist");

	// Create .panda_simulation directory
	std::string path(getenv("HOME"));
	path += "/.panda_simulation";
	ROS_INFO("Creating %s collision objects directory.", path);
	try
	{
	boost::filesystem::create_directory(path);
	}
	catch (const std::exception&)
	{
	ROS_ERROR(
	"%s directory could not be created."
	"Please create this directory yourself "
	"if you want to specify collision objects.", path.c_str());
	return -1;
	}
	}

	std::vector<moveit_msgs::CollisionObject> collision_objects;
	ROS_INFO_STREAM(app_directory << " is a directory containing:");
	for (auto &entry : boost::make_iterator_range(fs::directory_iterator(app_directory), {}))
	{
	ROS_INFO_STREAM(entry);

	std::ifstream file_stream(entry.path().string(), std::ifstream::binary);
	if (file_stream)
	{
	Json::Value root;
	file_stream >> root;

	moveit_msgs::CollisionObject collision_object = extractObstacleFromJson(root, entry.path().stem().string());
	collision_objects.push_back(collision_object);
	}
	else
	{
	ROS_WARN_STREAM("could not open file " << entry.path());
	}
	}

	// Publish the collision objects to the scene
	for (const auto &collision_object : collision_objects)
	{
	collision_object.header.frame_id = move_group_arm.getPlanningFrame();
	planning_scene.world.collision_objects.push_back(collision_object);
	}

	ROS_INFO_STREAM("# collision objects " << planning_scene.world.collision_objects.size());
	planning_scene.is_diff = true;
	planning_scene_diff_publisher.publish(planning_scene);

	ROS_INFO("robot_control_node is ready");
	ros::waitForShutdown();

	return 0;
	}
	}
	No newline at end of file

src/robot_state_initializer.cpp

+6 −6

Original line number	Original line	Diff line number	Diff line
	@@ -20,7 +20,7 @@ int main(int argc, char **argv) {
	ros::Duration(2.0).sleep();		ros::Duration(2.0).sleep();

	// 1. switch from default joint trajectory controller to custom position controller		// 1. switch from default joint trajectory controller to custom position controller
	std::string panda_arm_controller{"panda_arm_controller"}, panda_hand_controller{"panda_hand_controller"},		std::string panda_arm_controller{"position_joint_trajectory_controller"}, panda_hand_controller{"panda_hand_controller"},
	joint_position_controller{"joint_position_controller"};		joint_position_controller{"joint_position_controller"};
	std::vector<std::string> stop_controllers{panda_arm_controller, panda_hand_controller};		std::vector<std::string> stop_controllers{panda_arm_controller, panda_hand_controller};
	std::vector<std::string> start_controllers{joint_position_controller};		std::vector<std::string> start_controllers{joint_position_controller};
	@@ -62,11 +62,11 @@ int main(int argc, char **argv) {
	srv_switch_controller.request.start_controllers = start_controllers;		srv_switch_controller.request.start_controllers = start_controllers;

	if (switch_controller_client.call(srv_switch_controller)) {		if (switch_controller_client.call(srv_switch_controller)) {
	ROS_INFO_STREAM("Success switching controllers from " << stream_stop_controllers.str() << " to "		ROS_INFO_STREAM("Success switching controllers from " << stream_start_controllers.str() << " to "
	<< stream_start_controllers.str());		<< stream_stop_controllers.str());
	} else {		} else {
	ROS_WARN_STREAM("Error switching controllers from " << stream_stop_controllers.str() << " to "		ROS_WARN_STREAM("Error switching controllers from " << stream_start_controllers.str() << " to "
	<< stream_start_controllers.str());		<< stream_stop_controllers.str());
	return -1;		return -1;
	}		}

worlds/simulator.world

0 → 100644

+21 −0

Original line number	Original line	Diff line number	Diff line
			<?xml version="1.0" ?>
			<sdf version="1.5">
			<world name="default">
			<!-- A global light source -->
			<include>
			<uri>model://sun</uri>
			</include>
			<!-- A ground plane -->
			<include>
			<uri>model://ground_plane</uri>
			</include>
			<physics name='default_physics' default='0' type='ode'>
			<!-- the max step size is set to 0.001 by default. -->
			<!-- When increasing it, the simulation quality may suffer. -->
			<max_step_size>0.002</max_step_size>
			<!-- this factor actually is computed by multiplying the other two -->
			<real_time_factor>0.2</real_time_factor>
			<real_time_update_rate>100</real_time_update_rate>
			</physics>
			</world>
			</sdf>

Compare revisions

Source

Target

Commits on Source 28

Files

.devcontainer/Dockerfile

.devcontainer/devcontainer.json

.editorconfig

CMakeLists.txt

README.md

assets/robot-state-initializer.gif

config/panda_control.yaml

launch/box.launch

launch/move_group_interface_tutorial.launch

launch/simulation.launch

mainpage.dox

models/box.xacro

package.xml

rosdoc.yaml

scripts/docker-setup.sh

src/box_publisher.cpp

src/move_group_interface_tutorial.cpp

src/robot_control_node.cpp

src/robot_state_initializer.cpp

worlds/simulator.world