Skip to main content
Explore
Sign in
Explore
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found
Select Git revision
  • main
1 result

Target

Select target project
  • GUT Teaching / introduction-to-robotics / Introduction To Robotics
  • Florian Becker / Introduction To Robotics
  • Raaz Mohilden / Introduction To Robotics
  • Max Huthmacher / Introduction To Robotics
  • Madhava Pandiyan Chellaiah Nagarajan / Introduction To Robotics
  • Tim Beckers / Introduction To Robotics
  • Mojdeh Gohari / Introduction To Robotics
7 results
Select Git revision
  • main
1 result
Show changes
74 files
+ 5502
− 38
Compare changes
  • Side-by-side
  • Inline

Files

.gitignore

+8 −1
Original line number Original line Diff line number Diff line
@@ -2,3 +2,10 @@ sol_* 
.ipynb_*

.ipynb_*

.DS_STORE*

.DS_STORE*

map.*

map.*

.vscode/



Exercises/catkin_ws/.catkin_tools/*

Exercises/catkin_ws/build/*

Exercises/catkin_ws/devel/*

Exercises/catkin_ws/logs/*

Exercises/catkin_ws/CMakeLists.txt
 
 No newline at end of file

Exercises/catkin_ws/src/gut_robot/arm_robot/CMakeLists.txt

0 → 100755
+29 −0
Original line number Original line Diff line number Diff line 
cmake_minimum_required(VERSION 3.1.3)



project(arm_robot)



find_package(catkin REQUIRED COMPONENTS

  roscpp

  rospy

  std_msgs

  message_generation

  geometry_msgs

  urdf

  xacro

)



catkin_package(

CATKIN_DEPENDS 

  message_runtime

  geometry_msgs

  roscpp 

  rospy 

  std_msgs

)



find_package(roslaunch)



foreach(dir config launch meshes urdf)

	install(DIRECTORY ${dir}/

		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})

endforeach(dir)

Exercises/catkin_ws/src/gut_robot/arm_robot/config/joint_names_arm_robot.yaml

0 → 100755
+1 −0
Original line number Original line Diff line number Diff line 
controller_joint_names: ['', 'joint1', 'joint2', 'joint3', 'joint4', 'virtual_roll_joint', 'virtual_yaw_joint', 'joint5', 'joint6', ]

Exercises/catkin_ws/src/gut_robot/arm_robot/config/joint_trajectory_controller.yaml

0 → 100755
+49 −0
Original line number Original line Diff line number Diff line 


gripper_group_controller:

  type: "position_controllers/JointTrajectoryController"

  joints: [joint5, joint6]



joint_state_controller:

  type: "joint_state_controller/JointStateController"

  publish_rate: 50



arm_group_controller:

  type: "effort_controllers/JointTrajectoryController"

  joints:

    - joint1

    - joint2

    - joint3

    - joint4

    - virtual_roll_joint

    - virtual_yaw_joint



  constraints:

    goal_time: 0.5                   # Override default

    stopped_velocity_tolerance: 0.02 # Override default

    joint1:

      trajectory: 0.05               # Not enforced if unspecified

      goal: 0.02                     # Not enforced if unspecified

    joint2:

      trajectory: 0.05               # Not enforced if unspecified

      goal: 0.02                     # Not enforced if unspecified

    joint3:

      trajectory: 0.05               # Not enforced if unspecified

      goal: 0.02                     # Not enforced if unspecified

    joint4:

      trajectory: 0.05               # Not enforced if unspecified

      goal: 0.02                     # Not enforced if unspecified

    virtual_roll_joint:

      trajectory: 0.05               # Not enforced if unspecified

      goal: 0.02                     # Not enforced if unspecified

    virtual_yaw_joint:

      trajectory: 0.05               # Not enforced if unspecified

      goal: 0.02                     # Not enforced if unspecified



  gains: # Required because we're controlling an effort interface

    joint1: {p: 50,  d: 0.01, i: 0, i_clamp: 1}

    joint2: {p: 100,  d: 0.01, i: 0, i_clamp: 1}

    joint3: {p: 100,  d: 0.01, i: 0, i_clamp: 1}

    joint4: {p: 100,  d: 0.01, i: 0, i_clamp: 1}

    virtual_roll_joint: {p: 100,  d: 0.01, i: 0, i_clamp: 1}

    virtual_yaw_joint: {p: 100,  d: 0.01, i: 0, i_clamp: 1}

Exercises/catkin_ws/src/gut_robot/arm_robot/export.log

0 → 100755
+739 KiB

File added.

Preview size limit exceeded, changes collapsed.

View file

Exercises/catkin_ws/src/gut_robot/arm_robot/launch/arm_urdf.launch

0 → 100755
+41 −0
Original line number Original line Diff line number Diff line 
<launch>

	<arg name="arg_x" default="0.00" />

	<arg name="arg_y" default="0.00" />

	<arg name="arg_z" default="0.00" />

	<arg name="arg_R" default="0.00" />

	<arg name="arg_P" default="0.00" />

	<arg name="arg_Y" default="0.00" />



	<!--Urdf

	file path-->

	<param name="robot_description" textfile="$(find arm_robot)/urdf/arm_robot.urdf" />



	<!--spawn

	a empty gazebo world-->

	<include file="$(find gazebo_ros)/launch/empty_world.launch" >

		<arg name="world_name" value="$(find arm_robot)/worlds/empty.world" />

		<arg name="paused" value="true" />

		<arg name="use_sim_time" value="true" />

	</include>

	<node name="tf_footprint_base" pkg="tf" type="static_transform_publisher"

		args="0 0 0 0 0 0 base_link base_footprint 40" />



	<!--spawn

	model-->

	<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"

		args="-x $(arg arg_x) -y $(arg arg_y) -z $(arg arg_z) -Y $(arg arg_Y) -param robot_description -urdf -model arm_robot

		 -J joint1 0.0 -J joint2 0.0 -J joint3 0.73 -J joint4 1.07 -J joint5 0.0 -J joint6 0.0 -unpause" />



	<!--Load

	and launch the joint trajectory controller-->

	<rosparam file="$(find arm_robot)/config/joint_trajectory_controller.yaml" command="load" />

	<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"

		output="screen" args="joint_state_controller arm_group_controller gripper_group_controller" />

	<!-- Robot State Publisher for TF of each joint: publishes all the current states of the joint,

	then RViz can visualize -->



	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"

		respawn="false" output="screen" />





</launch>
 
 No newline at end of file

Exercises/catkin_ws/src/gut_robot/arm_robot/launch/display.launch

0 → 100755
+20 −0
Original line number Original line Diff line number Diff line 
<launch>

  <arg

    name="model" />

  <param

    name="robot_description"

    textfile="$(find arm_robot)/urdf/arm_robot.urdf" />

  <node

    name="joint_state_publisher_gui"

    pkg="joint_state_publisher_gui"

    type="joint_state_publisher_gui" />

  <node

    name="robot_state_publisher"

    pkg="robot_state_publisher"

    type="robot_state_publisher" />

  <node

    name="rviz"

    pkg="rviz"

    type="rviz"

    args="-d $(find arm_robot)/urdf.rviz" />

</launch>
 
 No newline at end of file

Exercises/catkin_ws/src/gut_robot/arm_robot/launch/gazebo.launch

0 → 100755
+20 −0
Original line number Original line Diff line number Diff line 
<launch>

  <include

    file="$(find gazebo_ros)/launch/empty_world.launch" />

  <node

    name="tf_footprint_base"

    pkg="tf"

    type="static_transform_publisher"

    args="0 0 0 0 0 0 base_link base_footprint 40" />

  <node

    name="spawn_model"

    pkg="gazebo_ros"

    type="spawn_model"

    args="-file $(find arm_robot)/urdf/arm_robot.urdf -urdf -model arm_robot"

    output="screen" />

  <node

    name="fake_joint_calibration"

    pkg="rostopic"

    type="rostopic"

    args="pub /calibrated std_msgs/Bool true" />

</launch>
 
 No newline at end of file

Exercises/catkin_ws/src/gut_robot/arm_robot/meshes/base_link.STL

0 → 100755
+157 KiB

File added.

No diff preview for this file type.

View file

Exercises/catkin_ws/src/gut_robot/arm_robot/meshes/link1.STL

0 → 100755
+301 KiB

File added.

No diff preview for this file type.

View file

Exercises/catkin_ws/src/gut_robot/arm_robot/meshes/link2.STL

0 → 100755
+377 KiB

File added.

No diff preview for this file type.

View file

Exercises/catkin_ws/src/gut_robot/arm_robot/meshes/link3.STL

0 → 100755
+347 KiB

File added.

No diff preview for this file type.

View file

Exercises/catkin_ws/src/gut_robot/arm_robot/meshes/link4.STL

0 → 100755
+653 KiB

File added.

No diff preview for this file type.

View file

Exercises/catkin_ws/src/gut_robot/arm_robot/meshes/link5.STL

0 → 100755
+572 KiB

File added.

No diff preview for this file type.

View file

Exercises/catkin_ws/src/gut_robot/arm_robot/meshes/link6.STL

0 → 100755
+1.06 MiB

File added.

No diff preview for this file type.

View file

Exercises/catkin_ws/src/gut_robot/arm_robot/package.xml

0 → 100755
+49 −0
Original line number Original line Diff line number Diff line 
<package format="2">

  <name>arm_robot</name>

  <version>1.0.0</version>

  <description>

    <p>URDF Description package for arm_robot</p>

    <p>This package contains configuration data, 3D models and launch files

for arm_robot robot</p>

  </description>

  <author>A Das</author>

  <maintainer email="abhirup.das@rwth-aachen.de" />

  <license>BSD</license>

  <buildtool_depend>catkin</buildtool_depend>

  

  <build_depend>roscpp</build_depend>

  <build_depend>rospy</build_depend>

  <build_depend>std_msgs</build_depend>

  <build_depend>geometry_msgs</build_depend>

  <build_depend>urdf</build_depend>

  <build_depend>xacro</build_depend>

  <build_depend>message_generation</build_depend>

 

  <depend>roslaunch</depend>

  <depend>robot_state_publisher</depend>

  <depend>rviz</depend>

  <depend>joint_state_publisher</depend>

  <depend>joint_state_publisher_gui</depend>

  <depend>gazebo</depend>

  <depend>moveit_simple_controller_manager</depend>

  

  <build_export_depend>roscpp</build_export_depend>

  <build_export_depend>rospy</build_export_depend>

  <build_export_depend>std_msgs</build_export_depend>

  <build_export_depend>geometry_msgs</build_export_depend>

  <build_export_depend>urdf</build_export_depend>

  <build_export_depend>xacro</build_export_depend>

  <build_export_depend>message_generation</build_export_depend>

  

  <exec_depend>roscpp</exec_depend>

  <exec_depend>rospy</exec_depend>

  <exec_depend>std_msgs</exec_depend>

  <exec_depend>geometry_msgs</exec_depend>

  <exec_depend>urdf</exec_depend>

  <exec_depend>xacro</exec_depend>

  <exec_depend>message_runtime</exec_depend>

  

  <export>

    <architecture_independent />

  </export>

</package>

Exercises/catkin_ws/src/gut_robot/arm_robot/urdf.rviz

0 → 100755
+246 −0
Original line number Original line Diff line number Diff line 
Panels:

  - Class: rviz/Displays

    Help Height: 78

    Name: Displays

    Property Tree Widget:

      Expanded:

        - /Global Options1

        - /Status1

        - /TF1

        - /TF1/Frames1

      Splitter Ratio: 0.5

    Tree Height: 549

  - Class: rviz/Selection

    Name: Selection

  - Class: rviz/Tool Properties

    Expanded:

      - /2D Pose Estimate1

      - /2D Nav Goal1

      - /Publish Point1

    Name: Tool Properties

    Splitter Ratio: 0.5886790156364441

  - Class: rviz/Views

    Expanded:

      - /Current View1

    Name: Views

    Splitter Ratio: 0.5

  - Class: rviz/Time

    Name: Time

    SyncMode: 0

    SyncSource: ""

Preferences:

  PromptSaveOnExit: true

Toolbars:

  toolButtonStyle: 2

Visualization Manager:

  Class: ""

  Displays:

    - Alpha: 0.5

      Cell Size: 1

      Class: rviz/Grid

      Color: 160; 160; 164

      Enabled: true

      Line Style:

        Line Width: 0.029999999329447746

        Value: Lines

      Name: Grid

      Normal Cell Count: 0

      Offset:

        X: 0

        Y: 0

        Z: 0

      Plane: XY

      Plane Cell Count: 10

      Reference Frame: <Fixed Frame>

      Value: true

    - Alpha: 1

      Class: rviz/RobotModel

      Collision Enabled: false

      Enabled: true

      Links:

        All Links Enabled: true

        Expand Joint Details: false

        Expand Link Details: false

        Expand Tree: false

        Link Tree Style: Links in Alphabetic Order

        base_link:

          Alpha: 1

          Show Axes: false

          Show Trail: false

          Value: true

        gripper_center:

          Alpha: 1

          Show Axes: false

          Show Trail: false

        link1:

          Alpha: 1

          Show Axes: false

          Show Trail: false

          Value: true

        link2:

          Alpha: 1

          Show Axes: false

          Show Trail: false

          Value: true

        link3:

          Alpha: 1

          Show Axes: false

          Show Trail: false

          Value: true

        link4:

          Alpha: 1

          Show Axes: false

          Show Trail: false

          Value: true

        link5:

          Alpha: 1

          Show Axes: false

          Show Trail: false

          Value: true

        link6:

          Alpha: 1

          Show Axes: false

          Show Trail: false

          Value: true

        world:

          Alpha: 1

          Show Axes: false

          Show Trail: false

      Name: RobotModel

      Robot Description: robot_description

      TF Prefix: ""

      Update Interval: 0

      Value: true

      Visual Enabled: true

    - Class: rviz/TF

      Enabled: true

      Filter (blacklist): ""

      Filter (whitelist): ""

      Frame Timeout: 15

      Frames:

        All Enabled: false

        base_link:

          Value: false

        gripper_center:

          Value: true

        link1:

          Value: false

        link2:

          Value: false

        link3:

          Value: false

        link4:

          Value: true

        link5:

          Value: false

        link6:

          Value: false

        world:

          Value: false

      Marker Alpha: 1

      Marker Scale: 1

      Name: TF

      Show Arrows: true

      Show Axes: true

      Show Names: true

      Tree:

        world:

          base_link:

            link1:

              link2:

                link3:

                  link4:

                    gripper_center:

                      {}

                    link5:

                      {}

                    link6:

                      {}

      Update Interval: 0

      Value: true

  Enabled: true

  Global Options:

    Background Color: 48; 48; 48

    Default Light: true

    Fixed Frame: world

    Frame Rate: 30

  Name: root

  Tools:

    - Class: rviz/Interact

      Hide Inactive Objects: true

    - Class: rviz/MoveCamera

    - Class: rviz/Select

    - Class: rviz/FocusCamera

    - Class: rviz/Measure

    - Class: rviz/SetInitialPose

      Theta std deviation: 0.2617993950843811

      Topic: /initialpose

      X std deviation: 0.5

      Y std deviation: 0.5

    - Class: rviz/SetGoal

      Topic: /move_base_simple/goal

    - Class: rviz/PublishPoint

      Single click: true

      Topic: /clicked_point

  Value: true

  Views:

    Current:

      Class: rviz/Orbit

      Distance: 0.445334255695343

      Enable Stereo Rendering:

        Stereo Eye Separation: 0.05999999865889549

        Stereo Focal Distance: 1

        Swap Stereo Eyes: false

        Value: false

      Field of View: 0.7853981852531433

      Focal Point:

        X: 0.2058536857366562

        Y: 0.0014365644892677665

        Z: 0.34997329115867615

      Focal Shape Fixed Size: true

      Focal Shape Size: 0.05000000074505806

      Invert Z Axis: false

      Name: Current View

      Near Clip Distance: 0.009999999776482582

      Pitch: 0.36039838194847107

      Target Frame: <Fixed Frame>

      Yaw: 3.0803983211517334

    Saved:

      - Class: rviz/Orbit

        Distance: 0.445334255695343

        Enable Stereo Rendering:

          Stereo Eye Separation: 0.05999999865889549

          Stereo Focal Distance: 1

          Swap Stereo Eyes: false

          Value: false

        Field of View: 0.7853981852531433

        Focal Point:

          X: 0.2058536857366562

          Y: 0.0014365644892677665

          Z: 0.34997329115867615

        Focal Shape Fixed Size: true

        Focal Shape Size: 0.05000000074505806

        Invert Z Axis: false

        Name: Orbit

        Near Clip Distance: 0.009999999776482582

        Pitch: 0.36039838194847107

        Target Frame: <Fixed Frame>

        Yaw: 3.0803983211517334

Window Geometry:

  Displays:

    collapsed: false

  Height: 846

  Hide Left Dock: false

  Hide Right Dock: false

  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000

  Selection:

    collapsed: false

  Time:

    collapsed: false

  Tool Properties:

    collapsed: false

  Views:

    collapsed: false

  Width: 1200

  X: 60

  Y: 27

Exercises/catkin_ws/src/gut_robot/arm_robot/urdf/arm_robot.csv

0 → 100755
+8 −0
Original line number Original line Diff line number Diff line 
Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity

base_link,-0.00013508,0.021215,0.0003347,0,0,0,0.85623,0.0025365,-3.3034E-08,1.1719E-06,0.0050308,8.1853E-08,0.0025341,0,0,0,0,0,0,package://arm_robot/meshes/base_link.STL,0.79216,0.81961,0.93333,1,0,0,0,0,0,0,package://arm_robot/meshes/base_link.STL,,big_base-1,base_attachment-2,Part 1-1,base_attachment-3,base_attachment-1,mg995.stp-4,Part 3-1,new org,,,,0,0,0,0,0,0,,0,0,0,,,,,,

link1,0.0001723,0.010717,0.0033,0,0,0,0.17559,0.00017251,1.0427E-06,8.7306E-07,0.00031357,-2.0249E-06,0.00015248,0,0,0,0,0,0,package://arm_robot/meshes/link1.STL,0.79216,0.81961,0.93333,1,0,0,0,0,0,0,package://arm_robot/meshes/link1.STL,,robot_painter_02-arm motor clamp-1-solid1_rev-1,mg995.stp-3,twisting_link_base-1,Origin_joint1,Axis1,joint1,revolute,0,0.0653,0,0,1.4664,0,base_link,0,1,0,3,3,0,3.142,,,,,,

link2,0.0028854,-0.037657,0.02116,0,0,0,0.029075,3.2099E-05,-3.0364E-09,1.3417E-09,8.7678E-06,1.2187E-05,2.3608E-05,0,0,0,0,0,0,package://arm_robot/meshes/link2.STL,0.79216,0.81961,0.93333,1,0,0,0,0,0,0,package://arm_robot/meshes/link2.STL,,robot_painter_02-a1_2_painter-1-solid1-1,robot_painter_02-Circle-1-solid1-2,robot_painter_02-a1_painter-1-solid1-1,robot_painter_02-Circle-1-solid1-1,Origin_joint2,Axis2,joint2,revolute,0,0.0229,0.00043961,0.51193,0,3.1416,link1,-1,0,0,3,3,-1.57,1.57,,,,,

link3,0.039018,-1.5837E-05,0.00013351,0,0,0,0.077386,8.8164E-06,6.2986E-08,-1.6053E-06,6.4568E-05,-4.7787E-10,6.301E-05,0,0,0,0,0,0,package://arm_robot/meshes/link3.STL,0.79216,0.81961,0.93333,1,0,0,0,0,0,0,package://arm_robot/meshes/link3.STL,,mg995.stp-2,robot_painter_02-a2_2PRA-1-solid1-1,robot_painter_02-a2_1PRA-1-solid1-1,robot_painter_02-Circle-1-solid1-3,Origin_joint3,Axis3,joint3,revolute,0,-0.075315,0.042319,1.5708,-0.51193,-1.5708,link2,0,0,1,3,3,-1.57,1.57,,,,,

link4,0.075718,0.0078323,-0.0023511,0,0,0,0.12848,3.7512E-05,-2.5961E-08,3.1447E-06,5.7704E-05,-9.6946E-09,3.5473E-05,0,0,0,0,0,0,package://arm_robot/meshes/link4.STL,0.79216,0.81961,0.93333,1,0,0,0,0,0,0,package://arm_robot/meshes/link4.STL,,mg995.stp-1,robot_painter_02-a3_PRA-1-solid1-1,gripper_wrist-1,gripper_upper_palm-1,gripper_lower_palm-1,mg995.stp-5,Origin_joint4,Axis4,joint4,revolute,0.1181,0,0,0,0,0,link3,0,0,1,3,3,-1.57,1.57,,,

link5,0.027123,0.00037494,0.039116,0,0,0,0.01684,4.0476E-06,-6.256E-08,-2.0534E-06,5.368E-06,-1.0318E-07,1.9876E-06,0,0,0,0,0,0,package://arm_robot/meshes/link5.STL,0.79216,0.81961,0.93333,1,0,0,0,0,0,0,package://arm_robot/meshes/link5.STL,,robot_painter_02-Gripper_Assembly-gen-pt4-1,robot_painter_02-Gripper_Assembly-finger_pt1-1,robot_painter_02-Gripper_Assembly-gen-pt1-1,Origin_joint5,Axis5,joint5,revolute,0.11993,0,0.012424,-3.1112,-0.89161,0,link4,0,1,0,1,3,0,1.57,,,,,,

link6,-0.023433,8.3598E-05,0.030623,0,0,0,0.021692,4.4063E-06,7.7606E-08,2.5426E-06,6.5097E-06,-1.1332E-07,2.7995E-06,0,0,0,0,0,0,package://arm_robot/meshes/link6.STL,0.79216,0.81961,0.93333,1,0,0,0,0,0,0,package://arm_robot/meshes/link6.STL,,robot_painter_02-Gripper_Assembly-gen-pt2-1,robot_painter_02-Gripper_Assembly-gen-pt3-1,robot_painter_02-Gripper_Assembly-finger_pt2-1,Origin_joint6,Axis6,joint6,revolute,0.11993,0,-0.015576,0.030391,-0.81089,-3.1416,link4,0,1,0,1,3,-1.57,0,,,,,,

Exercises/catkin_ws/src/gut_robot/arm_robot/urdf/arm_robot.urdf

0 → 100755
+575 −0
Original line number Original line Diff line number Diff line 
<?xml version="1.0" encoding="utf-8"?>

<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by

Stephen Brawner (brawner@gmail.com) 

     Commit Version: 1.6.0-4-g7f85cfe  Build Version: 1.6.7995.38578

     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->

<robot

  name="arm_robot">

  <link name="world" />

  <link

    name="base_link">

    <inertial>

      <origin

        xyz="-0.00013508 0.021215 0.0003347"

        rpy="0 0 0" />

      <mass

        value="0.85623" />

      <inertia

        ixx="0.0025365"

        ixy="-3.3034E-08"

        ixz="1.1719E-06"

        iyy="0.0050308"

        iyz="8.1853E-08"

        izz="0.0025341" />

    </inertial>

    <visual>

      <origin

        xyz="0 0 0"

        rpy="0 0 0" />

      <geometry>

        <mesh

          filename="package://arm_robot/meshes/base_link.STL" />

      </geometry>

      <material

        name="">

        <color

          rgba="0.79216 0.81961 0.93333 1" />

      </material>

    </visual>

    <collision>

      <origin

        xyz="0 0 0"

        rpy="0 0 0" />

      <geometry>

        <mesh

          filename="package://arm_robot/meshes/base_link.STL" />

      </geometry>

    </collision>

  </link>

  <joint name="base_joint" type="fixed">

    <parent link="world" />

    <child link="base_link" />

    <origin xyz="0.0 0.0 0.0" rpy="1.57 0 0" />

  </joint>

  <link

    name="link1">

    <inertial>

      <origin

        xyz="0.0001723 0.010717 0.0033"

        rpy="0 0 0" />

      <mass

        value="0.17559" />

      <inertia

        ixx="0.00017251"

        ixy="1.0427E-06"

        ixz="8.7306E-07"

        iyy="0.00031357"

        iyz="-2.0249E-06"

        izz="0.00015248" />

    </inertial>

    <visual>

      <origin

        xyz="0 0 0"

        rpy="0 0 0" />

      <geometry>

        <mesh

          filename="package://arm_robot/meshes/link1.STL" />

      </geometry>

      <material

        name="">

        <color

          rgba="0.79216 0.81961 0.93333 1" />

      </material>

    </visual>

    <collision>

      <origin

        xyz="0 0 0"

        rpy="0 0 0" />

      <geometry>

        <mesh

          filename="package://arm_robot/meshes/link1.STL" />

      </geometry>

    </collision>

  </link>

  <joint

    name="joint1"

    type="revolute">

    <origin

      xyz="0 0.0653 0"

      rpy="0 1.4664 0" />

    <parent

      link="base_link" />

    <child

      link="link1" />

    <axis

      xyz="0 1 0" />

    <limit

      lower="0"

      upper="3.142"

      effort="3"

      velocity="4" />

  </joint>

  <link

    name="link2">

    <inertial>

      <origin

        xyz="0.0028854 -0.037657 0.02116"

        rpy="0 0 0" />

      <mass

        value="0.029075" />

      <inertia

        ixx="3.2099E-05"

        ixy="-3.0364E-09"

        ixz="1.3417E-09"

        iyy="8.7678E-06"

        iyz="1.2187E-05"

        izz="2.3608E-05" />

    </inertial>

    <visual>

      <origin

        xyz="0 0 0"

        rpy="0 0 0" />

      <geometry>

        <mesh

          filename="package://arm_robot/meshes/link2.STL" />

      </geometry>

      <material

        name="">

        <color

          rgba="0.79216 0.81961 0.93333 1" />

      </material>

    </visual>

    <collision>

      <origin

        xyz="0 0 0"

        rpy="0 0 0" />

      <geometry>

        <mesh

          filename="package://arm_robot/meshes/link2.STL" />

      </geometry>

    </collision>

  </link>

  <joint

    name="joint2"

    type="revolute">

    <origin

      xyz="0 0.0229 0.00043961"

      rpy="0.51193 0 3.1416" />

    <parent

      link="link1" />

    <child

      link="link2" />

    <axis

      xyz="-1 0 0" />

    <limit

      lower="-1.57"

      upper="1.57"

      effort="3"

      velocity="4" />

  </joint>

  <link

    name="link3">

    <inertial>

      <origin

        xyz="0.039018 -1.5837E-05 0.00013351"

        rpy="0 0 0" />

      <mass

        value="0.077386" />

      <inertia

        ixx="8.8164E-06"

        ixy="6.2986E-08"

        ixz="-1.6053E-06"

        iyy="6.4568E-05"

        iyz="-4.7787E-10"

        izz="6.301E-05" />

    </inertial>

    <visual>

      <origin

        xyz="0 0 0"

        rpy="0 0 0" />

      <geometry>

        <mesh

          filename="package://arm_robot/meshes/link3.STL" />

      </geometry>

      <material

        name="">

        <color

          rgba="0.79216 0.81961 0.93333 1" />

      </material>

    </visual>

    <collision>

      <origin

        xyz="0 0 0"

        rpy="0 0 0" />

      <geometry>

        <mesh

          filename="package://arm_robot/meshes/link3.STL" />

      </geometry>

    </collision>

  </link>

  <joint

    name="joint3"

    type="revolute">

    <origin

      xyz="0 -0.075315 0.042319"

      rpy="1.5708 -0.51193 -1.5708" />

    <parent

      link="link2" />

    <child

      link="link3" />

    <axis

      xyz="0 0 1" />

    <limit

      lower="-1.57"

      upper="1.57"

      effort="3"

      velocity="4" />

  </joint>

  <link

    name="link4">

    <inertial>

      <origin

        xyz="0.075718 0.0078323 -0.0023511"

        rpy="0 0 0" />

      <mass

        value="0.12848" />

      <inertia

        ixx="3.7512E-05"

        ixy="-2.5961E-08"

        ixz="3.1447E-06"

        iyy="5.7704E-05"

        iyz="-9.6946E-09"

        izz="3.5473E-05" />

    </inertial>

    <visual>

      <origin

        xyz="0 0 0"

        rpy="0 0 0" />

      <geometry>

        <mesh

          filename="package://arm_robot/meshes/link4.STL" />

      </geometry>

      <material

        name="">

        <color

          rgba="0.79216 0.81961 0.93333 1" />

      </material>

    </visual>

    <collision>

      <origin

        xyz="0 0 0"

        rpy="0 0 0" />

      <geometry>

        <mesh

          filename="package://arm_robot/meshes/link4.STL" />

      </geometry>

    </collision>

  </link>

  <joint

    name="joint4"

    type="revolute">

    <origin

      xyz="0.1181 0 0"

      rpy="0 0 0" />

    <parent

      link="link3" />

    <child

      link="link4" />

    <axis

      xyz="0 0 1" />

    <limit

      lower="-1.57"

      upper="1.57"

      effort="3"

      velocity="4" />

  </joint>

  <link

    name="link5">

    <inertial>

      <origin

        xyz="0.027123 0.00037494 0.039116"

        rpy="0 0 0" />

      <mass

        value="0.01684" />

      <inertia

        ixx="4.0476E-06"

        ixy="-6.256E-08"

        ixz="-2.0534E-06"

        iyy="5.368E-06"

        iyz="-1.0318E-07"

        izz="1.9876E-06" />

    </inertial>

    <visual>

      <origin

        xyz="0 0 0"

        rpy="0 0 0" />

      <geometry>

        <mesh

          filename="package://arm_robot/meshes/link5.STL" />

      </geometry>

      <material

        name="">

        <color

          rgba="0.79216 0.81961 0.93333 1" />

      </material>

    </visual>

    <collision>

      <origin

        xyz="0 0 0"

        rpy="0 0 0" />

      <geometry>

        <mesh

          filename="package://arm_robot/meshes/link5.STL" />

      </geometry>

    </collision>

  </link>

  <joint

    name="joint5"

    type="revolute">

    <origin

      xyz="0.11993 0 0.012424"

      rpy="-3.1112 -0.89161 0" />

    <parent

      link="link4" />

    <child

      link="link5" />

    <axis

      xyz="0 1 0" />

    <limit

      lower="0"

      upper="1.57"

      effort="1"

      velocity="4" />

  </joint>

  <link

    name="link6">

    <inertial>

      <origin

        xyz="-0.023433 8.3598E-05 0.030623"

        rpy="0 0 0" />

      <mass

        value="0.021692" />

      <inertia

        ixx="4.4063E-06"

        ixy="7.7606E-08"

        ixz="2.5426E-06"

        iyy="6.5097E-06"

        iyz="-1.1332E-07"

        izz="2.7995E-06" />

    </inertial>

    <visual>

      <origin

        xyz="0 0 0"

        rpy="0 0 0" />

      <geometry>

        <mesh

          filename="package://arm_robot/meshes/link6.STL" />

      </geometry>

      <material

        name="">

        <color

          rgba="0.79216 0.81961 0.93333 1" />

      </material>

    </visual>

    <collision>

      <origin

        xyz="0 0 0"

        rpy="0 0 0" />

      <geometry>

        <mesh

          filename="package://arm_robot/meshes/link6.STL" />

      </geometry>

    </collision>

  </link>

  <joint

    name="joint6"

    type="revolute">

    <origin

      xyz="0.11993 0 -0.015576"

      rpy="0.030391 -0.81089 -3.1416" />

    <parent

      link="link4" />

    <child

      link="link6" />

    <axis

      xyz="0 1 0" />

    <limit

      lower="-1.57"

      upper="0"

      effort="1"

      velocity="4" />

  </joint>

  <!-- Virtual roll joint -->

  <joint name="virtual_roll_joint" type="revolute">

    <limit lower="-3.1415" upper="3.1415" effort="54.0" velocity="4" />

    <parent link="link4" />

    <child link="virtual_roll_link" />

    <axis xyz="1 0 0" />

    <origin xyz="0.2056 0.0 0.0" rpy="0 0 0" />

    <dynamics damping="0.0" friction="0.0" />

  </joint>



  <!-- Virtual roll link -->

  <link name="virtual_roll_link">

    <visual>

      <origin xyz="0 0 0" rpy="0 0 0" />

      <geometry>

        <box size="0.001 0.001 0.001" />

      </geometry>

    </visual>



    <inertial>

      <origin xyz="0 0 0" />

      <mass value="1.0e-03" />

      <inertia ixx="1.0e-03" ixy="0.0" ixz="0.0"

        iyy="1.0e-03" iyz="0.0"

        izz="1.0e-03" />

    </inertial>

  </link>



  <gazebo reference="virtual_roll_link">

  </gazebo>



  <!-- Virtual yaw joint -->

  <joint name="virtual_yaw_joint" type="revolute">

    <limit lower="-3.1415" upper="3.1415" effort="54.0" velocity="4" />

    <parent link="virtual_roll_link" />

    <child link="virtual_yaw_link" />

    <axis xyz="0 0 1" />

    <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />

    <dynamics damping="0.0" friction="0.0" />

  </joint>



  <!-- Virtual yaw link -->

  <link name="virtual_yaw_link">

    <visual>

      <origin xyz="0 0 0" rpy="0 0 0" />

      <geometry>

        <box size="0.001 0.001 0.001" />

      </geometry>

    </visual>



    <inertial>

      <origin xyz="0 0 0" />

      <mass value="1.0e-03" />

      <inertia ixx="1.0e-03" ixy="0.0" ixz="0.0"

        iyy="1.0e-03" iyz="0.0"

        izz="1.0e-03" />

    </inertial>

  </link>

  <gazebo reference="virtual_yaw_link">

  </gazebo>



  <!-- end effector joint -->

  <link name="gripper_center">

  </link>

  <joint name="gripper_center_joint" type="fixed">

    <parent link="virtual_yaw_link" />

    <child link="gripper_center" />

    <origin xyz="0.0 0.0 0.0" rpy="-1.57 0.0 -1.57" />

  </joint>



  <transmission name="link1_trans">

    <type>transmission_interface/SimpleTransmission</type>

    <joint name="joint1">

      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

    </joint>

    <actuator name="link1_motor">

      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

      <mechanicalReduction>1</mechanicalReduction>

    </actuator>

  </transmission>

  <transmission name="link2_trans">

    <type>transmission_interface/SimpleTransmission</type>

    <joint name="joint2">

      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

    </joint>

    <actuator name="link2_motor">

      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

      <mechanicalReduction>1</mechanicalReduction>

    </actuator>

  </transmission>

  <transmission name="link3_trans">

    <type>transmission_interface/SimpleTransmission</type>

    <joint name="joint3">

      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

    </joint>

    <actuator name="link3_motor">

      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

      <mechanicalReduction>1</mechanicalReduction>

    </actuator>

  </transmission>

  <transmission name="link4_trans">

    <type>transmission_interface/SimpleTransmission</type>

    <joint name="joint4">

      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

    </joint>

    <actuator name="link4_motor">

      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

      <mechanicalReduction>1</mechanicalReduction>

    </actuator>

  </transmission>

  <transmission name="link5_trans">

    <type>transmission_interface/SimpleTransmission</type>

    <joint name="joint5">

      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

    </joint>

    <actuator name="link5_motor">

      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

      <mechanicalReduction>1</mechanicalReduction>

    </actuator>

  </transmission>

  <transmission name="link6_trans">

    <type>transmission_interface/SimpleTransmission</type>

    <joint name="joint6">

      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

    </joint>

    <actuator name="link6_motor">

      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

      <mechanicalReduction>1</mechanicalReduction>

    </actuator>

  </transmission>

  <transmission name="virtual_roll_joint_trans">

    <type>transmission_interface/SimpleTransmission</type>

    <joint name="virtual_roll_joint">

      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

    </joint>

    <actuator name="virtual_roll_joint_motor">

      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

      <mechanicalReduction>1</mechanicalReduction>

    </actuator>

  </transmission>

  <transmission name="virtual_yaw_joint_trans">

    <type>transmission_interface/SimpleTransmission</type>

    <joint name="virtual_yaw_joint">

      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

    </joint>

    <actuator name="virtual_yaw_joint_motor">

      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

      <mechanicalReduction>1</mechanicalReduction>

    </actuator>

  </transmission>

  <gazebo>

    <plugin name="control" filename="libgazebo_ros_control.so">

      <robotNamespace>/</robotNamespace>

    </plugin>

  </gazebo>

  <gazebo reference="link1">

    <selfCollide>true</selfCollide>

  </gazebo>

  <gazebo reference="link2">

    <selfCollide>true</selfCollide>

  </gazebo>

  <gazebo reference="link3">

    <selfCollide>true</selfCollide>

  </gazebo>

  <gazebo reference="link4">

    <selfCollide>true</selfCollide>

  </gazebo>

  <gazebo reference="link5">

    <selfCollide>true</selfCollide>

  </gazebo>

  <gazebo reference="link6">

    <selfCollide>true</selfCollide>

  </gazebo>

</robot>
 
 No newline at end of file

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/.setup_assistant

0 → 100755
+11 −0
Original line number Original line Diff line number Diff line 
moveit_setup_assistant_config:

  URDF:

    package: arm_robot

    relative_path: urdf/arm_robot.urdf

    xacro_args: ""

  SRDF:

    relative_path: config/arm_robot.srdf

  CONFIG:

    author_name: Abhirup Das

    author_email: abhirup.das@rwth-aachen.de

    generated_timestamp: 1736878867
 
 No newline at end of file

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/CMakeLists.txt

0 → 100755
+20 −0
Original line number Original line Diff line number Diff line 
cmake_minimum_required(VERSION 3.1.3)

project(arm_robot_moveit)



find_package(catkin REQUIRED COMPONENTS

  roscpp

  rospy

  std_msgs

  message_generation

)



catkin_package(

	INCLUDE_DIRS

	CATKIN_DEPENDS

		roscpp

		std_msgs

)



install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}

  PATTERN "setup_assistant.launch" EXCLUDE)

install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/config/arm_robot.srdf

0 → 100755
+93 −0
Original line number Original line Diff line number Diff line 
<?xml version="1.0" encoding="UTF-8"?>

<!--This does not replace URDF, and is not an extension of URDF.

    This is a format for representing semantic information about the robot structure.

    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined

-->

<robot name="arm_robot">

    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->

    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->

    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->

    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->

    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->

    <group name="arm_group">

        <joint name="joint1"/>

        <joint name="joint2"/>

        <joint name="joint3"/>

        <joint name="joint4"/>

        <joint name="virtual_roll_joint"/>

        <joint name="virtual_yaw_joint"/>

        <joint name="gripper_center_joint"/>

    </group>

    <group name="gripper_group">

        <joint name="joint6"/>

        <joint name="joint5"/>

    </group>

    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->

    <group_state name="arm_home" group="arm_group">

        <joint name="joint1" value="1.57"/>

        <joint name="joint2" value="-0.785"/>

        <joint name="joint3" value="1.57"/>

        <joint name="joint4" value="0.785"/>

        <joint name="virtual_roll_joint" value="0"/>

        <joint name="virtual_yaw_joint" value="0"/>

    </group_state>

    <group_state name="gripper_open" group="gripper_group">

        <joint name="joint5" value="1.309"/>

        <joint name="joint6" value="-1.309"/>

    </group_state>

    <group_state name="gripper_close" group="gripper_group">

        <joint name="joint5" value="0.436"/>

        <joint name="joint6" value="-0.436"/>

    </group_state>

    <group_state name="pick_pose_1" group="arm_group">

        <joint name="joint1" value="0"/>

        <joint name="joint2" value="-0.348"/>

        <joint name="joint3" value="1.042"/>

        <joint name="joint4" value="1.57"/>

        <joint name="virtual_roll_joint" value="0"/>

        <joint name="virtual_yaw_joint" value="0"/>

    </group_state>

    <group_state name="pre_pick_pose_1" group="arm_group">

        <joint name="joint1" value="0"/>

        <joint name="joint2" value="-0.785"/>

        <joint name="joint3" value="1.57"/>

        <joint name="joint4" value="0.785"/>

        <joint name="virtual_roll_joint" value="0"/>

        <joint name="virtual_yaw_joint" value="0"/>

    </group_state>

    <group_state name="pre_place_pose" group="arm_group">

        <joint name="joint1" value="3.142"/>

        <joint name="joint2" value="-0.785"/>

        <joint name="joint3" value="1.57"/>

        <joint name="joint4" value="0.785"/>

        <joint name="virtual_roll_joint" value="0"/>

        <joint name="virtual_yaw_joint" value="0"/>

    </group_state>

    <group_state name="place_pose" group="arm_group">

        <joint name="joint1" value="3.142"/>

        <joint name="joint2" value="-0.347"/>

        <joint name="joint3" value="1.042"/>

        <joint name="joint4" value="1.57"/>

        <joint name="virtual_roll_joint" value="0"/>

        <joint name="virtual_yaw_joint" value="0"/>

    </group_state>

    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->

    <end_effector name="gripper" parent_link="link4" group="gripper_group" parent_group="arm_group"/>

    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->

    <disable_collisions link1="base_link" link2="link1" reason="Adjacent"/>

    <disable_collisions link1="link1" link2="link2" reason="Adjacent"/>

    <disable_collisions link1="link1" link2="link3" reason="Never"/>

    <disable_collisions link1="link1" link2="link4" reason="Never"/>

    <disable_collisions link1="link1" link2="link5" reason="Never"/>

    <disable_collisions link1="link1" link2="link6" reason="Never"/>

    <disable_collisions link1="link2" link2="link3" reason="Adjacent"/>

    <disable_collisions link1="link2" link2="link4" reason="Never"/>

    <disable_collisions link1="link2" link2="link5" reason="Never"/>

    <disable_collisions link1="link2" link2="link6" reason="Never"/>

    <disable_collisions link1="link3" link2="link4" reason="Adjacent"/>

    <disable_collisions link1="link3" link2="link5" reason="Never"/>

    <disable_collisions link1="link3" link2="link6" reason="Never"/>

    <disable_collisions link1="link4" link2="link5" reason="Adjacent"/>

    <disable_collisions link1="link4" link2="link6" reason="Adjacent"/>

    <disable_collisions link1="link5" link2="link6" reason="Default"/>

</robot>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/config/cartesian_limits.yaml

0 → 100755
+5 −0
Original line number Original line Diff line number Diff line 
cartesian_limits:

  max_trans_vel: 1

  max_trans_acc: 2.25

  max_trans_dec: -5

  max_rot_vel: 1.57

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/config/chomp_planning.yaml

0 → 100755
+18 −0
Original line number Original line Diff line number Diff line 
planning_time_limit: 10.0

max_iterations: 200

max_iterations_after_collision_free: 5

smoothness_cost_weight: 0.1

obstacle_cost_weight: 1.0

learning_rate: 0.01

smoothness_cost_velocity: 0.0

smoothness_cost_acceleration: 1.0

smoothness_cost_jerk: 0.0

ridge_factor: 0.0

use_pseudo_inverse: false

pseudo_inverse_ridge_factor: 1e-4

joint_update_limit: 0.1

collision_clearance: 0.2

collision_threshold: 0.07

use_stochastic_descent: true

enable_failure_recovery: false

max_recovery_attempts: 5

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/config/fake_controllers.yaml

0 → 100755
+20 −0
Original line number Original line Diff line number Diff line 
controller_list:

  - name: fake_arm_group_controller

    type: $(arg fake_execution_type)

    joints:

      - joint1

      - joint2

      - joint3

      - joint4

      - virtual_roll_joint

      - virtual_yaw_joint

  - name: fake_gripper_group_controller

    type: $(arg fake_execution_type)

    joints:

      - joint5

      - joint6

initial:  # Define initial robot poses per group

  - group: arm_group

    pose: arm_home

  - group: gripper_group

    pose: gripper_close
 
 No newline at end of file

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/config/gazebo_arm_robot.urdf

0 → 100755
+375 −0
Original line number Original line Diff line number Diff line 
<?xml version="1.0" encoding="utf-8" ?>

<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 

     Commit Version: 1.6.0-4-g7f85cfe  Build Version: 1.6.7995.38578

     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->

<robot name="arm_robot">

    <link name="world" />

    <link name="base_link">

        <inertial>

            <origin xyz="-0.00013508 0.021215 0.0003347" rpy="0 0 0" />

            <mass value="0.85623" />

            <inertia ixx="0.0025365" ixy="-3.3034E-08" ixz="1.1719E-06" iyy="0.0050308" iyz="8.1853E-08" izz="0.0025341" />

        </inertial>

        <visual>

            <origin xyz="0 0 0" rpy="0 0 0" />

            <geometry>

                <mesh filename="package://arm_robot/meshes/base_link.STL" />

            </geometry>

            <material name="">

                <color rgba="0.79216 0.81961 0.93333 1" />

            </material>

        </visual>

        <collision>

            <origin xyz="0 0 0" rpy="0 0 0" />

            <geometry>

                <mesh filename="package://arm_robot/meshes/base_link.STL" />

            </geometry>

        </collision>

    </link>

    <joint name="base_joint" type="fixed">

        <parent link="world" />

        <child link="base_link" />

        <origin xyz="0.0 0.0 0.0" rpy="1.57 0 0" />

    </joint>

    <link name="link1">

        <inertial>

            <origin xyz="0.0001723 0.010717 0.0033" rpy="0 0 0" />

            <mass value="0.17559" />

            <inertia ixx="0.00017251" ixy="1.0427E-06" ixz="8.7306E-07" iyy="0.00031357" iyz="-2.0249E-06" izz="0.00015248" />

        </inertial>

        <visual>

            <origin xyz="0 0 0" rpy="0 0 0" />

            <geometry>

                <mesh filename="package://arm_robot/meshes/link1.STL" />

            </geometry>

            <material name="">

                <color rgba="0.79216 0.81961 0.93333 1" />

            </material>

        </visual>

        <collision>

            <origin xyz="0 0 0" rpy="0 0 0" />

            <geometry>

                <mesh filename="package://arm_robot/meshes/link1.STL" />

            </geometry>

        </collision>

    </link>

    <joint name="joint1" type="revolute">

        <origin xyz="0 0.0653 0" rpy="0 1.4664 0" />

        <parent link="base_link" />

        <child link="link1" />

        <axis xyz="0 1 0" />

        <limit lower="0" upper="3.142" effort="3" velocity="4" />

    </joint>

    <link name="link2">

        <inertial>

            <origin xyz="0.0028854 -0.037657 0.02116" rpy="0 0 0" />

            <mass value="0.029075" />

            <inertia ixx="3.2099E-05" ixy="-3.0364E-09" ixz="1.3417E-09" iyy="8.7678E-06" iyz="1.2187E-05" izz="2.3608E-05" />

        </inertial>

        <visual>

            <origin xyz="0 0 0" rpy="0 0 0" />

            <geometry>

                <mesh filename="package://arm_robot/meshes/link2.STL" />

            </geometry>

            <material name="">

                <color rgba="0.79216 0.81961 0.93333 1" />

            </material>

        </visual>

        <collision>

            <origin xyz="0 0 0" rpy="0 0 0" />

            <geometry>

                <mesh filename="package://arm_robot/meshes/link2.STL" />

            </geometry>

        </collision>

    </link>

    <joint name="joint2" type="revolute">

        <origin xyz="0 0.0229 0.00043961" rpy="0.51193 0 3.1416" />

        <parent link="link1" />

        <child link="link2" />

        <axis xyz="-1 0 0" />

        <limit lower="-1.57" upper="1.57" effort="3" velocity="4" />

    </joint>

    <link name="link3">

        <inertial>

            <origin xyz="0.039018 -1.5837E-05 0.00013351" rpy="0 0 0" />

            <mass value="0.077386" />

            <inertia ixx="8.8164E-06" ixy="6.2986E-08" ixz="-1.6053E-06" iyy="6.4568E-05" iyz="-4.7787E-10" izz="6.301E-05" />

        </inertial>

        <visual>

            <origin xyz="0 0 0" rpy="0 0 0" />

            <geometry>

                <mesh filename="package://arm_robot/meshes/link3.STL" />

            </geometry>

            <material name="">

                <color rgba="0.79216 0.81961 0.93333 1" />

            </material>

        </visual>

        <collision>

            <origin xyz="0 0 0" rpy="0 0 0" />

            <geometry>

                <mesh filename="package://arm_robot/meshes/link3.STL" />

            </geometry>

        </collision>

    </link>

    <joint name="joint3" type="revolute">

        <origin xyz="0 -0.075315 0.042319" rpy="1.5708 -0.51193 -1.5708" />

        <parent link="link2" />

        <child link="link3" />

        <axis xyz="0 0 1" />

        <limit lower="-1.57" upper="1.57" effort="3" velocity="4" />

    </joint>

    <link name="link4">

        <inertial>

            <origin xyz="0.075718 0.0078323 -0.0023511" rpy="0 0 0" />

            <mass value="0.12848" />

            <inertia ixx="3.7512E-05" ixy="-2.5961E-08" ixz="3.1447E-06" iyy="5.7704E-05" iyz="-9.6946E-09" izz="3.5473E-05" />

        </inertial>

        <visual>

            <origin xyz="0 0 0" rpy="0 0 0" />

            <geometry>

                <mesh filename="package://arm_robot/meshes/link4.STL" />

            </geometry>

            <material name="">

                <color rgba="0.79216 0.81961 0.93333 1" />

            </material>

        </visual>

        <collision>

            <origin xyz="0 0 0" rpy="0 0 0" />

            <geometry>

                <mesh filename="package://arm_robot/meshes/link4.STL" />

            </geometry>

        </collision>

    </link>

    <joint name="joint4" type="revolute">

        <origin xyz="0.1181 0 0" rpy="0 0 0" />

        <parent link="link3" />

        <child link="link4" />

        <axis xyz="0 0 1" />

        <limit lower="-1.57" upper="1.57" effort="3" velocity="4" />

    </joint>

    <link name="link5">

        <inertial>

            <origin xyz="0.027123 0.00037494 0.039116" rpy="0 0 0" />

            <mass value="0.01684" />

            <inertia ixx="4.0476E-06" ixy="-6.256E-08" ixz="-2.0534E-06" iyy="5.368E-06" iyz="-1.0318E-07" izz="1.9876E-06" />

        </inertial>

        <visual>

            <origin xyz="0 0 0" rpy="0 0 0" />

            <geometry>

                <mesh filename="package://arm_robot/meshes/link5.STL" />

            </geometry>

            <material name="">

                <color rgba="0.79216 0.81961 0.93333 1" />

            </material>

        </visual>

        <collision>

            <origin xyz="0 0 0" rpy="0 0 0" />

            <geometry>

                <mesh filename="package://arm_robot/meshes/link5.STL" />

            </geometry>

        </collision>

    </link>

    <joint name="joint5" type="revolute">

        <origin xyz="0.11993 0 0.012424" rpy="-3.1112 -0.89161 0" />

        <parent link="link4" />

        <child link="link5" />

        <axis xyz="0 1 0" />

        <limit lower="0" upper="1.57" effort="1" velocity="4" />

    </joint>

    <link name="link6">

        <inertial>

            <origin xyz="-0.023433 8.3598E-05 0.030623" rpy="0 0 0" />

            <mass value="0.021692" />

            <inertia ixx="4.4063E-06" ixy="7.7606E-08" ixz="2.5426E-06" iyy="6.5097E-06" iyz="-1.1332E-07" izz="2.7995E-06" />

        </inertial>

        <visual>

            <origin xyz="0 0 0" rpy="0 0 0" />

            <geometry>

                <mesh filename="package://arm_robot/meshes/link6.STL" />

            </geometry>

            <material name="">

                <color rgba="0.79216 0.81961 0.93333 1" />

            </material>

        </visual>

        <collision>

            <origin xyz="0 0 0" rpy="0 0 0" />

            <geometry>

                <mesh filename="package://arm_robot/meshes/link6.STL" />

            </geometry>

        </collision>

    </link>

    <joint name="joint6" type="revolute">

        <origin xyz="0.11993 0 -0.015576" rpy="0.030391 -0.81089 -3.1416" />

        <parent link="link4" />

        <child link="link6" />

        <axis xyz="0 1 0" />

        <limit lower="-1.57" upper="0" effort="1" velocity="4" />

    </joint>

    <!-- Virtual roll joint -->

    <joint name="virtual_roll_joint" type="revolute">

        <limit lower="-3.1415" upper="3.1415" effort="54.0" velocity="4" />

        <parent link="link4" />

        <child link="virtual_roll_link" />

        <axis xyz="1 0 0" />

        <origin xyz="0.2056 0.0 0.0" rpy="0 0 0" />

        <dynamics damping="0.0" friction="0.0" />

    </joint>

    <!-- Virtual roll link -->

    <link name="virtual_roll_link">

        <visual>

            <origin xyz="0 0 0" rpy="0 0 0" />

            <geometry>

                <box size="0.001 0.001 0.001" />

            </geometry>

        </visual>

        <inertial>

            <origin xyz="0 0 0" />

            <mass value="1.0e-03" />

            <inertia ixx="1.0e-03" ixy="0.0" ixz="0.0" iyy="1.0e-03" iyz="0.0" izz="1.0e-03" />

        </inertial>

    </link>

    <gazebo reference="virtual_roll_link">

        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">

            <robotNamespace>/</robotNamespace>

        </plugin>

    </gazebo>

    <!-- Virtual yaw joint -->

    <joint name="virtual_yaw_joint" type="revolute">

        <limit lower="-3.1415" upper="3.1415" effort="54.0" velocity="4" />

        <parent link="virtual_roll_link" />

        <child link="virtual_yaw_link" />

        <axis xyz="0 0 1" />

        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />

        <dynamics damping="0.0" friction="0.0" />

    </joint>

    <!-- Virtual yaw link -->

    <link name="virtual_yaw_link">

        <visual>

            <origin xyz="0 0 0" rpy="0 0 0" />

            <geometry>

                <box size="0.001 0.001 0.001" />

            </geometry>

        </visual>

        <inertial>

            <origin xyz="0 0 0" />

            <mass value="1.0e-03" />

            <inertia ixx="1.0e-03" ixy="0.0" ixz="0.0" iyy="1.0e-03" iyz="0.0" izz="1.0e-03" />

        </inertial>

    </link>

    <gazebo reference="virtual_yaw_link" />

    <!-- end effector joint -->

    <link name="gripper_center">

        <visual>

            <origin xyz="0 0 0" rpy="0 0 0" />

        </visual>

    </link>

    <joint name="gripper_center_joint" type="fixed">

        <parent link="virtual_yaw_link" />

        <child link="gripper_center" />

        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />

    </joint>

    <transmission name="link1_trans">

        <type>transmission_interface/SimpleTransmission</type>

        <joint name="joint1">

            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

        </joint>

        <actuator name="link1_motor">

            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

            <mechanicalReduction>1</mechanicalReduction>

        </actuator>

    </transmission>

    <transmission name="link2_trans">

        <type>transmission_interface/SimpleTransmission</type>

        <joint name="joint2">

            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

        </joint>

        <actuator name="link2_motor">

            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

            <mechanicalReduction>1</mechanicalReduction>

        </actuator>

    </transmission>

    <transmission name="link3_trans">

        <type>transmission_interface/SimpleTransmission</type>

        <joint name="joint3">

            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

        </joint>

        <actuator name="link3_motor">

            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

            <mechanicalReduction>1</mechanicalReduction>

        </actuator>

    </transmission>

    <transmission name="link4_trans">

        <type>transmission_interface/SimpleTransmission</type>

        <joint name="joint4">

            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

        </joint>

        <actuator name="link4_motor">

            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

            <mechanicalReduction>1</mechanicalReduction>

        </actuator>

    </transmission>

    <transmission name="link5_trans">

        <type>transmission_interface/SimpleTransmission</type>

        <joint name="joint5">

            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

        </joint>

        <actuator name="link5_motor">

            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

            <mechanicalReduction>1</mechanicalReduction>

        </actuator>

    </transmission>

    <transmission name="link6_trans">

        <type>transmission_interface/SimpleTransmission</type>

        <joint name="joint6">

            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

        </joint>

        <actuator name="link6_motor">

            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

            <mechanicalReduction>1</mechanicalReduction>

        </actuator>

    </transmission>

    <transmission name="virtual_roll_joint_trans">

        <type>transmission_interface/SimpleTransmission</type>

        <joint name="virtual_roll_joint">

            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

        </joint>

        <actuator name="virtual_roll_joint_motor">

            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

            <mechanicalReduction>1</mechanicalReduction>

        </actuator>

    </transmission>

    <transmission name="virtual_yaw_joint_trans">

        <type>transmission_interface/SimpleTransmission</type>

        <joint name="virtual_yaw_joint">

            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

        </joint>

        <actuator name="virtual_yaw_joint_motor">

            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>

            <mechanicalReduction>1</mechanicalReduction>

        </actuator>

    </transmission>

    <gazebo>

        <plugin name="control" filename="libgazebo_ros_control.so">

            <robotNamespace>/</robotNamespace>

        </plugin>

    </gazebo>

    <gazebo reference="link1">

        <selfCollide>true</selfCollide>

    </gazebo>

    <gazebo reference="link2">

        <selfCollide>true</selfCollide>

    </gazebo>

    <gazebo reference="link3">

        <selfCollide>true</selfCollide>

    </gazebo>

    <gazebo reference="link4">

        <selfCollide>true</selfCollide>

    </gazebo>

    <gazebo reference="link5">

        <selfCollide>true</selfCollide>

    </gazebo>

    <gazebo reference="link6">

        <selfCollide>true</selfCollide>

    </gazebo>

</robot>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/config/gazebo_controllers.yaml

0 → 100755
+4 −0
Original line number Original line Diff line number Diff line 
# Publish joint_states

joint_state_controller:

  type: joint_state_controller/JointStateController

  publish_rate: 50

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/config/joint_limits.yaml

0 → 100755
+50 −0
Original line number Original line Diff line number Diff line 
# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed



# For beginners, we downscale velocity and acceleration limits.

# You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.

default_velocity_scaling_factor: 0.5

default_acceleration_scaling_factor: 0.1



# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]

# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]

joint_limits:

  joint1:

    has_velocity_limits: true

    max_velocity: 4

    has_acceleration_limits: false

    max_acceleration: 0

  joint2:

    has_velocity_limits: true

    max_velocity: 4

    has_acceleration_limits: false

    max_acceleration: 0

  joint3:

    has_velocity_limits: true

    max_velocity: 4

    has_acceleration_limits: false

    max_acceleration: 0

  joint4:

    has_velocity_limits: true

    max_velocity: 4

    has_acceleration_limits: false

    max_acceleration: 0

  joint5:

    has_velocity_limits: true

    max_velocity: 4

    has_acceleration_limits: false

    max_acceleration: 0

  joint6:

    has_velocity_limits: true

    max_velocity: 4

    has_acceleration_limits: false

    max_acceleration: 0

  virtual_roll_joint:

    has_velocity_limits: true

    max_velocity: 4

    has_acceleration_limits: false

    max_acceleration: 0

  virtual_yaw_joint:

    has_velocity_limits: true

    max_velocity: 4

    has_acceleration_limits: false

    max_acceleration: 0

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/config/kinematics.yaml

0 → 100755
+6 −0
Original line number Original line Diff line number Diff line 
arm_group:

  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin

  kinematics_solver_search_resolution: 0.005

  kinematics_solver_timeout: 0.005

  goal_joint_tolerance: 0.0001

  goal_position_tolerance: 0.0001
 
 No newline at end of file

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/config/ompl_planning.yaml

0 → 100755
+229 −0
Original line number Original line Diff line number Diff line 
planner_configs:

  AnytimePathShortening:

    type: geometric::AnytimePathShortening

    shortcut: true  # Attempt to shortcut all new solution paths

    hybridize: true  # Compute hybrid solution trajectories

    max_hybrid_paths: 24  # Number of hybrid paths generated per iteration

    num_planners: 4  # The number of default planners to use for planning

    planners: ""  # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]"

  SBL:

    type: geometric::SBL

    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()

  EST:

    type: geometric::EST

    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()

    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05

  LBKPIECE:

    type: geometric::LBKPIECE

    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()

    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9

    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5

  BKPIECE:

    type: geometric::BKPIECE

    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()

    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9

    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5

    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5

  KPIECE:

    type: geometric::KPIECE

    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()

    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05

    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9 (0.0,1.]

    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5

    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5

  RRT:

    type: geometric::RRT

    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()

    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05

  RRTConnect:

    type: geometric::RRTConnect

    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()

  RRTstar:

    type: geometric::RRTstar

    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()

    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05

    delay_collision_checking: 1  # Stop collision checking as soon as C-free parent found. default 1

  TRRT:

    type: geometric::TRRT

    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()

    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05

    max_states_failed: 10  # when to start increasing temp. default: 10

    temp_change_factor: 2.0  # how much to increase or decrease temp. default: 2.0

    min_temperature: 10e-10  # lower limit of temp change. default: 10e-10

    init_temperature: 10e-6  # initial temperature. default: 10e-6

    frontier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()

    frontier_node_ratio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1

    k_constant: 0.0  # value used to normalize expresssion. default: 0.0 set in setup()

  PRM:

    type: geometric::PRM

    max_nearest_neighbors: 10  # use k nearest neighbors. default: 10

  PRMstar:

    type: geometric::PRMstar

  FMT:

    type: geometric::FMT

    num_samples: 1000  # number of states that the planner should sample. default: 1000

    radius_multiplier: 1.1  # multiplier used for the nearest neighbors search radius. default: 1.1

    nearest_k: 1  # use Knearest strategy. default: 1

    cache_cc: 1  # use collision checking cache. default: 1

    heuristics: 0  # activate cost to go heuristics. default: 0

    extended_fmt: 1  # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1

  BFMT:

    type: geometric::BFMT

    num_samples: 1000  # number of states that the planner should sample. default: 1000

    radius_multiplier: 1.0  # multiplier used for the nearest neighbors search radius. default: 1.0

    nearest_k: 1  # use the Knearest strategy. default: 1

    balanced: 0  # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1

    optimality: 1  # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1

    heuristics: 1  # activates cost to go heuristics. default: 1

    cache_cc: 1  # use the collision checking cache. default: 1

    extended_fmt: 1  # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1

  PDST:

    type: geometric::PDST

  STRIDE:

    type: geometric::STRIDE

    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()

    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05

    use_projected_distance: 0  # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0

    degree: 16  # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16

    max_degree: 18  # max degree of a node in the GNAT. default: 12

    min_degree: 12  # min degree of a node in the GNAT. default: 12

    max_pts_per_leaf: 6  # max points per leaf in the GNAT. default: 6

    estimated_dimension: 0.0  # estimated dimension of the free space. default: 0.0

    min_valid_path_fraction: 0.2  # Accept partially valid moves above fraction. default: 0.2

  BiTRRT:

    type: geometric::BiTRRT

    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()

    temp_change_factor: 0.1  # how much to increase or decrease temp. default: 0.1

    init_temperature: 100  # initial temperature. default: 100

    frontier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()

    frontier_node_ratio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1

    cost_threshold: 1e300  # the cost threshold. Any motion cost that is not better will not be expanded. default: inf

  LBTRRT:

    type: geometric::LBTRRT

    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()

    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05

    epsilon: 0.4  # optimality approximation factor. default: 0.4

  BiEST:

    type: geometric::BiEST

    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()

  ProjEST:

    type: geometric::ProjEST

    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()

    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05

  LazyPRM:

    type: geometric::LazyPRM

    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()

  LazyPRMstar:

    type: geometric::LazyPRMstar

  SPARS:

    type: geometric::SPARS

    stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0

    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25

    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001

    max_failures: 1000  # maximum consecutive failure limit. default: 1000

  SPARStwo:

    type: geometric::SPARStwo

    stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0

    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25

    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001

    max_failures: 5000  # maximum consecutive failure limit. default: 5000

  AITstar:

    type: geometric::AITstar

    use_k_nearest: 1  # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1

    rewire_factor: 1.001  # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001

    samples_per_batch: 100  # batch size. Valid values: [1:1:1000]. Default: 100

    use_graph_pruning: 1  # enable graph pruning (1) or not (0). Default: 1

    find_approximate_solutions: 0  # track approximate solutions (1) or not (0). Default: 0

    set_max_num_goals: 1  # maximum number of goals sampled from sampleable goal regions. Valid values: [1:1:1000]. Default: 1

  ABITstar:

    type: geometric::ABITstar

    use_k_nearest: 1  # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1

    rewire_factor: 1.001  # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001

    samples_per_batch: 100  # batch size. Valid values: [1:1:1000]. Default: 100

    use_graph_pruning: 1  # enable graph pruning (1) or not (0). Default: 1

    prune_threshold_as_fractional_cost_change: 0.1  # fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1

    delay_rewiring_to_first_solution: 0  # delay (1) or not (0) rewiring until a solution is found. Default: 0

    use_just_in_time_sampling: 0  # delay the generation of samples until they are * necessary. Only works with r-disc connection and path length minimization. Default: 0

    drop_unconnected_samples_on_prune: 0  # drop unconnected samples when pruning, regardless of their heuristic value. Default: 0

    stop_on_each_solution_improvement: 0  # stop the planner each time a solution improvement is found. Useful for debugging. Default: 0

    use_strict_queue_ordering: 0  # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0

    find_approximate_solutions: 0  # track approximate solutions (1) or not (0). Default: 0

    initial_inflation_factor: 1000000  # inflation factor for the initial search. Valid values: [1.0:0.01:1000000.0]. Default: 1000000

    inflation_scaling_parameter: 10  # scaling parameter for the inflation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0

    truncation_scaling_parameter: 5.0  # scaling parameter for the truncation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0

  BITstar:

    type: geometric::BITstar

    use_k_nearest: 1  # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1

    rewire_factor: 1.001  # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001

    samples_per_batch: 100  # batch size. Valid values: [1:1:1000]. Default: 100

    use_graph_pruning: 1  # enable graph pruning (1) or not (0). Default: 1

    prune_threshold_as_fractional_cost_change: 0.1  # fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1

    delay_rewiring_to_first_solution: 0  # delay (1) or not (0) rewiring until a solution is found. Default: 0

    use_just_in_time_sampling: 0  # delay the generation of samples until they are * necessary. Only works with r-disc connection and path length minimization. Default: 0

    drop_unconnected_samples_on_prune: 0  # drop unconnected samples when pruning, regardless of their heuristic value. Default: 0

    stop_on_each_solution_improvement: 0  # stop the planner each time a solution improvement is found. Useful for debugging. Default: 0

    use_strict_queue_ordering: 0  # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0

    find_approximate_solutions: 0  # track approximate solutions (1) or not (0). Default: 0

arm_group:

  default_planner_config: RRTConnect

  planner_configs:

    - AnytimePathShortening

    - SBL

    - EST

    - LBKPIECE

    - BKPIECE

    - KPIECE

    - RRT

    - RRTConnect

    - RRTstar

    - TRRT

    - PRM

    - PRMstar

    - FMT

    - BFMT

    - PDST

    - STRIDE

    - BiTRRT

    - LBTRRT

    - BiEST

    - ProjEST

    - LazyPRM

    - LazyPRMstar

    - SPARS

    - SPARStwo

    - AITstar

    - ABITstar

    - BITstar

  projection_evaluator: joints(joint1,joint2)

  longest_valid_segment_fraction: 0.005

gripper_group:

  planner_configs:

    - AnytimePathShortening

    - SBL

    - EST

    - LBKPIECE

    - BKPIECE

    - KPIECE

    - RRT

    - RRTConnect

    - RRTstar

    - TRRT

    - PRM

    - PRMstar

    - FMT

    - BFMT

    - PDST

    - STRIDE

    - BiTRRT

    - LBTRRT

    - BiEST

    - ProjEST

    - LazyPRM

    - LazyPRMstar

    - SPARS

    - SPARStwo

    - AITstar

    - ABITstar

    - BITstar

  projection_evaluator: joints(joint5,joint6)

  longest_valid_segment_fraction: 0.005

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/config/ros_controllers.yaml

0 → 100755
+64 −0
Original line number Original line Diff line number Diff line 
moveit_sim_hw_interface:

  joint_model_group: arm_group

  joint_model_group_pose: arm_home

# Settings for ros_control_boilerplate control loop

generic_hw_control_loop:

  loop_hz: 300

  cycle_time_error_threshold: 0.01

# Settings for ros_control hardware interface

hardware_interface:

  joints:

    - joint1

    - joint2

    - joint3

    - joint4

    - gripper

    - virtual_roll_joint

    - virtual_yaw_joint

  sim_control_mode: 1  # 0: position, 1: velocity





joint_state_controller:

  type: joint_state_controller/JointStateController

  publish_rate: 50

controller_list:

  - name: arm_group_controller

    action_ns: follow_joint_trajectory

    default: True

    type: FollowJointTrajectory

    joints:

      - joint1

      - joint2

      - joint3

      - joint4

      - virtual_roll_joint

      - virtual_yaw_joint

    gains:

      joint1:

        p: 100.0

        i: 0.0

        d: 10.0

        i_clamp: 1.0  # Optional: limit for integral term

      joint2:

        p: 100.0

        i: 0.0

        d: 10.0

        i_clamp: 1.0

      joint3:

        p: 100.0

        i: 0.0

        d: 10.0

        i_clamp: 1.0

      joint4:

        p: 100.0

        i: 0.0

        d: 10.0

        i_clamp: 1.0



  - name: gripper_group_controller

    action_ns: follow_joint_trajectory

    default: True

    type: FollowJointTrajectory

    joints:

      - joint5

      - joint6
 
 No newline at end of file

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/config/sensors_3d.yaml

0 → 100755
+2 −0
Original line number Original line Diff line number Diff line 
sensors:

  []
 
 No newline at end of file

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/config/simple_moveit_controllers.yaml

0 → 100755
+20 −0
Original line number Original line Diff line number Diff line 
controller_list:

  - name: arm_group_controller

    action_ns: follow_joint_trajectory

    type: FollowJointTrajectory

    default: true

    joints:

    - joint1

    - joint2

    - joint3

    - joint4

    - virtual_roll_joint

    - virtual_yaw_joint

  

  - name: gripper_group_controller

    action_ns: follow_joint_trajectory

    default: True

    type: FollowJointTrajectory

    joints:

      - joint5

      - joint6
 
 No newline at end of file

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/config/stomp_planning.yaml

0 → 100755
+78 −0
Original line number Original line Diff line number Diff line 
stomp/arm_group:

  group_name: arm_group

  optimization:

    num_timesteps: 60

    num_iterations: 40

    num_iterations_after_valid: 0

    num_rollouts: 30

    max_rollouts: 30

    initialization_method: 1  # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST]

    control_cost_weight: 0.0

  task:

    noise_generator:

      - class: stomp_moveit/NormalDistributionSampling

        stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05]

    cost_functions:

      - class: stomp_moveit/CollisionCheck

        collision_penalty: 1.0

        cost_weight: 1.0

        kernel_window_percentage: 0.2

        longest_valid_joint_move: 0.05

    noisy_filters:

      - class: stomp_moveit/JointLimits

        lock_start: True

        lock_goal: True

      - class: stomp_moveit/MultiTrajectoryVisualization

        line_width: 0.02

        rgb: [255, 255, 0]

        marker_array_topic: stomp_trajectories

        marker_namespace: noisy

    update_filters:

      - class: stomp_moveit/PolynomialSmoother

        poly_order: 6

      - class: stomp_moveit/TrajectoryVisualization

        line_width: 0.05

        rgb: [0, 191, 255]

        error_rgb: [255, 0, 0]

        publish_intermediate: True

        marker_topic: stomp_trajectory

        marker_namespace: optimized

stomp/gripper_group:

  group_name: gripper_group

  optimization:

    num_timesteps: 60

    num_iterations: 40

    num_iterations_after_valid: 0

    num_rollouts: 30

    max_rollouts: 30

    initialization_method: 1  # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST]

    control_cost_weight: 0.0

  task:

    noise_generator:

      - class: stomp_moveit/NormalDistributionSampling

        stddev: [0.05, 0.05]

    cost_functions:

      - class: stomp_moveit/CollisionCheck

        collision_penalty: 1.0

        cost_weight: 1.0

        kernel_window_percentage: 0.2

        longest_valid_joint_move: 0.05

    noisy_filters:

      - class: stomp_moveit/JointLimits

        lock_start: True

        lock_goal: True

      - class: stomp_moveit/MultiTrajectoryVisualization

        line_width: 0.02

        rgb: [255, 255, 0]

        marker_array_topic: stomp_trajectories

        marker_namespace: noisy

    update_filters:

      - class: stomp_moveit/PolynomialSmoother

        poly_order: 6

      - class: stomp_moveit/TrajectoryVisualization

        line_width: 0.05

        rgb: [0, 191, 255]

        error_rgb: [255, 0, 0]

        publish_intermediate: True

        marker_topic: stomp_trajectory

        marker_namespace: optimized
 
 No newline at end of file

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/arm_robot_moveit_sensor_manager.launch.xml

0 → 100755
+3 −0
Original line number Original line Diff line number Diff line 
<launch>



</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/chomp_planning_pipeline.launch.xml

0 → 100755
+28 −0
Original line number Original line Diff line number Diff line 
<launch>

  <arg name="start_state_max_bounds_error" default="0.1" />

  <arg name="jiggle_fraction" default="0.05" />

  <!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->

  <!-- <arg name="planning_adapters"

       default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed

                default_planner_request_adapters/AddTimeParameterization

                default_planner_request_adapters/ResolveConstraintFrames

                default_planner_request_adapters/FixWorkspaceBounds

                default_planner_request_adapters/FixStartStateBounds

                default_planner_request_adapters/FixStartStateCollision

                default_planner_request_adapters/FixStartStatePathConstraints"

                /> -->

  <arg name="planning_adapters"

    value="default_planner_request_adapters/AddTimeParameterization

		 default_planner_request_adapters/FixWorkspaceBounds

		 default_planner_request_adapters/FixStartStateBounds

		 default_planner_request_adapters/FixStartStateCollision

		 default_planner_request_adapters/FixStartStatePathConstraints"

  />



  <param name="planning_plugin" value="chomp_interface/CHOMPPlanner" />

  <param name="request_adapters" value="$(arg planning_adapters)" />

  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />

  <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />



  <rosparam command="load" file="$(find arm_robot_moveit)/config/chomp_planning.yaml" />

</launch>
 
 No newline at end of file

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/default_warehouse_db.launch

0 → 100755
+15 −0
Original line number Original line Diff line number Diff line 
<launch>



  <arg name="reset" default="false"/>

  <!-- If not specified, we'll use a default database location -->

  <arg name="moveit_warehouse_database_path" default="$(find arm_robot_moveit)/default_warehouse_mongo_db" />



  <!-- Launch the warehouse with the configured database location -->

  <include file="$(dirname)/warehouse.launch">

    <arg name="moveit_warehouse_database_path" value="$(arg moveit_warehouse_database_path)" />

  </include>



  <!-- If we want to reset the database, run this node -->

  <node if="$(arg reset)" name="$(anon moveit_default_db_reset)" type="moveit_init_demo_warehouse" pkg="moveit_ros_warehouse" respawn="false" output="screen" />



</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/demo.launch

0 → 100755
+66 −0
Original line number Original line Diff line number Diff line 
<launch>



  <!-- specify the planning pipeline -->

  <arg name="pipeline" default="ompl" />



  <!-- By default, we do not start a database (it can be large) -->

  <arg name="db" default="false" />

  <!-- Allow user to specify database location -->

  <arg name="db_path" default="$(find arm_robot_moveit)/default_warehouse_mongo_db" />



  <!-- By default, we are not in debug mode -->

  <arg name="debug" default="false" />



  <!-- By default, we will load or override the robot_description -->

  <arg name="load_robot_description" default="true"/>



  <!-- Choose controller manager: fake, simple, or ros_control -->

  <arg name="moveit_controller_manager" default="fake" />

  <!-- Set execution mode for fake execution controllers -->

  <arg name="fake_execution_type" default="interpolate" />



  <!-- By default, hide joint_state_publisher's GUI in 'fake' controller_manager mode -->

  <arg name="use_gui" default="false" />

  <arg name="use_rviz" default="true" />



  <!-- If needed, broadcast static tf for robot root -->



  <group if="$(eval arg('moveit_controller_manager') == 'fake')">

    <!-- We do not have a real robot connected, so publish fake joint states via a joint_state_publisher

         MoveIt's fake controller's joint states are considered via the 'source_list' parameter -->

    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">

      <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>

    </node>

    <!-- If desired, a GUI version is available allowing to move the simulated robot around manually

         This corresponds to moving around the real robot without the use of MoveIt. -->

    <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">

      <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>

    </node>



    <!-- Given the published joint states, publish tf for the robot links -->

    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />

  </group>



  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->

  <include file="$(dirname)/move_group.launch">

    <arg name="allow_trajectory_execution" value="true"/>

    <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />

    <arg name="fake_execution_type" value="$(arg fake_execution_type)"/>

    <arg name="info" value="true"/>

    <arg name="debug" value="$(arg debug)"/>

    <arg name="pipeline" value="$(arg pipeline)"/>

    <arg name="load_robot_description" value="$(arg load_robot_description)"/>

  </include>



  <!-- Run Rviz and load the default config to see the state of the move_group node -->

  <include file="$(dirname)/moveit_rviz.launch" if="$(arg use_rviz)">

    <arg name="rviz_config" value="$(dirname)/moveit.rviz"/>

    <arg name="debug" value="$(arg debug)"/>

  </include>



  <!-- If database loading was enabled, start mongodb as well -->

  <include file="$(dirname)/default_warehouse_db.launch" if="$(arg db)">

    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>

  </include>



</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/demo_gazebo.launch

0 → 100755
+21 −0
Original line number Original line Diff line number Diff line 
<?xml version="1.0"?>

<launch>

  <!-- MoveIt options -->

  <arg name="pipeline" default="ompl" doc="Planning pipeline to use with MoveIt"/>



  <!-- Gazebo options -->

  <arg name="gazebo_gui" default="true" doc="Start Gazebo GUI"/>

  <arg name="paused" default="false" doc="Start Gazebo paused"/>

  <arg name="world_name" default="worlds/empty.world" doc="Gazebo world file"/>

  <arg name="world_pose" default="-x 0 -y 0 -z 0 -R 0 -P 0 -Y 0" doc="Pose to spawn the robot at"/>



  <!-- Launch Gazebo and spawn the robot -->

  <include file="$(dirname)/gazebo.launch" pass_all_args="true"/>



  <!-- Launch MoveIt -->

  <include file="$(dirname)/demo.launch" pass_all_args="true">

    <!-- robot_description is loaded by gazebo.launch, to enable Gazebo features -->

    <arg name="load_robot_description" value="false" />

    <arg name="moveit_controller_manager" value="ros_control" />

  </include>

</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/fake_moveit_controller_manager.launch.xml

0 → 100755
+12 −0
Original line number Original line Diff line number Diff line 
<launch>



  <!-- execute the trajectory in 'interpolate' mode or jump to goal position in 'last point' mode -->

  <arg name="fake_execution_type" default="interpolate" />



  <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->

  <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>



  <!-- The rest of the params are specific to this plugin -->

  <rosparam subst_value="true" file="$(find arm_robot_moveit)/config/fake_controllers.yaml"/>



</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/full_arm_robot_sim.launch

0 → 100755
+14 −0
Original line number Original line Diff line number Diff line 
<launch>

  <!-- Launch Your robot arms launch file which loads the robot in Gazebo and spawns the controllers -->

  <include file = "$(find arm_robot)/launch/arm_urdf.launch" />

  

  <!-- Launch Moveit Move Group Node -->

  <include file = "$(find arm_robot_moveit)/launch/move_group.launch" />

  

  <!-- Run Rviz and load the default configuration to see the state of the move_group node -->

  <arg name="use_rviz" default="true" />

  

  <include file="$(find arm_robot_moveit)/launch/moveit_rviz.launch" if="$(arg use_rviz)">

    <arg name="rviz_config" value="$(find arm_robot_moveit)/launch/moveit.rviz"/>

  </include>

</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/gazebo.launch

0 → 100755
+34 −0
Original line number Original line Diff line number Diff line 
<?xml version="1.0"?>

<launch>

  <!-- Gazebo options -->

  <arg name="gazebo_gui" default="true" doc="Start Gazebo GUI"/>

  <arg name="paused" default="false" doc="Start Gazebo paused"/>

  <arg name="world_name" default="worlds/empty.world" doc="Gazebo world file"/>

  <arg name="world_pose" default="-x 0 -y 0 -z 0 -R 0 -P 0 -Y 0" doc="Pose to spawn the robot at"/>

  <arg name="initial_joint_positions" default=" -J joint1 0 -J joint2 -0.785 -J joint3 1.57 -J joint4 0.785 -J joint5 0.785 -J joint6 -0.785 -J virtual_roll_joint 0 -J virtual_yaw_joint 0" doc="Initial joint configuration of the robot"/>



  <!-- Start Gazebo paused to allow the controllers to pickup the initial pose -->

  <include file="$(find gazebo_ros)/launch/empty_world.launch" pass_all_args="true">

    <arg name="paused" value="true"/>

  </include>



  <!-- Set the robot urdf on the parameter server -->

  <param name="robot_description" textfile="$(find arm_robot_moveit)/config/gazebo_arm_robot.urdf" />



  <!-- Unpause the simulation after loading the robot model -->

  <arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />



  <!-- Spawn the robot in Gazebo -->

  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot $(arg unpause) $(arg world_pose) $(arg initial_joint_positions)"

    respawn="false" output="screen" />



  <!-- Load the controller parameters onto the parameter server -->

  <rosparam file="$(find arm_robot_moveit)/config/gazebo_controllers.yaml" />

  <include file="$(dirname)/ros_controllers.launch"/>



  <!-- Spawn the Gazebo ROS controllers -->

  <node name="gazebo_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller" />



  <!-- Given the published joint states, publish tf for the robot links -->

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />

</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/joystick_control.launch

0 → 100755
+17 −0
Original line number Original line Diff line number Diff line 
<launch>

  <!-- See moveit_ros/visualization/doc/joystick.rst for documentation -->



  <arg name="dev" default="/dev/input/js0" />



  <!-- Launch joy node -->

  <node pkg="joy" type="joy_node" name="joy">

    <param name="dev" value="$(arg dev)" /> <!-- Customize this to match the location your joystick is plugged in on-->

    <param name="deadzone" value="0.2" />

    <param name="autorepeat_rate" value="40" />

    <param name="coalesce_interval" value="0.025" />

  </node>



  <!-- Launch python interface -->

  <node pkg="moveit_ros_visualization" type="moveit_joy.py" output="screen" name="moveit_joy"/>



</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/move_group.launch

0 → 100755
+105 −0
Original line number Original line Diff line number Diff line 
<launch>



  <!-- GDB Debug Option -->

  <arg name="debug" default="false" />

  <arg unless="$(arg debug)" name="launch_prefix" value="" />

  <arg     if="$(arg debug)" name="launch_prefix"

           value="gdb -x $(dirname)/gdb_settings.gdb --ex run --args" />



  <!-- Verbose Mode Option -->

  <arg name="info" default="$(arg debug)" />

  <arg unless="$(arg info)" name="command_args" value="" />

  <arg     if="$(arg info)" name="command_args" value="--debug" />



  <!-- move_group settings -->

  <arg name="pipeline" default="ompl" />

  <arg name="allow_trajectory_execution" default="true"/>

  <arg name="moveit_controller_manager" default="simple" />

  <arg name="fake_execution_type" default="interpolate"/>

  <arg name="max_safe_path_cost" default="1"/>

  <arg name="publish_monitored_planning_scene" default="true"/>



  <arg name="capabilities" default=""/>

  <arg name="disable_capabilities" default=""/>

  <!-- load these non-default MoveGroup capabilities (space seperated) -->

  <!--

  <arg name="capabilities" value="

                a_package/AwsomeMotionPlanningCapability

                another_package/GraspPlanningPipeline

                " />

  -->



  <!-- inhibit these default MoveGroup capabilities (space seperated) -->

  <!--

  <arg name="disable_capabilities" value="

                move_group/MoveGroupKinematicsService

                move_group/ClearOctomapService

                " />

  -->



  <arg name="load_robot_description" default="false" />

  <!-- load URDF, SRDF and joint_limits configuration -->

  <include file="$(dirname)/planning_context.launch">

    <arg name="load_robot_description" value="$(arg load_robot_description)" />

  </include>



  <!-- Planning Pipelines -->

  <group ns="move_group/planning_pipelines">



    <!-- OMPL -->

    <include file="$(dirname)/planning_pipeline.launch.xml">

      <arg name="pipeline" value="ompl" />

    </include>



    <!-- CHOMP -->

    <include file="$(dirname)/planning_pipeline.launch.xml">

      <arg name="pipeline" value="chomp" />

    </include>



    <!-- Pilz Industrial Motion -->

    <include file="$(dirname)/planning_pipeline.launch.xml">

      <arg name="pipeline" value="pilz_industrial_motion_planner" />

    </include>



    <!-- Support custom planning pipeline -->

    <include if="$(eval arg('pipeline') not in ['ompl', 'chomp', 'pilz_industrial_motion_planner'])"

             file="$(dirname)/planning_pipeline.launch.xml">

      <arg name="pipeline" value="$(arg pipeline)" />

    </include>

  </group>



  <!-- Trajectory Execution Functionality -->

  <include ns="move_group" file="$(dirname)/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">

    <arg name="moveit_manage_controllers" value="true" />

    <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />

    <arg name="fake_execution_type" value="$(arg fake_execution_type)" />

  </include>



  <!-- Sensors Functionality -->

  <include ns="move_group" file="$(dirname)/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">

    <arg name="moveit_sensor_manager" value="arm_robot" />

  </include>



  <!-- Start the actual move_group node/action server -->

  <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">

    <!-- Set the display variable, in case OpenGL code is used internally -->

    <env name="DISPLAY" value="$(optenv DISPLAY :0)" />



    <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>

    <param name="sense_for_plan/max_safe_path_cost" value="$(arg max_safe_path_cost)"/>

    <param name="default_planning_pipeline" value="$(arg pipeline)" />

    <param name="capabilities" value="$(arg capabilities)" />

    <param name="disable_capabilities" value="$(arg disable_capabilities)" />



    <!-- do not copy dynamics information from /joint_states to internal robot monitoring

         default to false, because almost nothing in move_group relies on this information -->

    <param name="monitor_dynamics" value="false" />



    <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->

    <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />

    <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />

    <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />

    <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />

  </node>



</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/moveit.rviz

0 → 100755
+342 −0
Original line number Original line Diff line number Diff line 
Panels:

  - Class: rviz/Displays

    Help Height: 0

    Name: Displays

    Property Tree Widget:

      Expanded:

        - /MotionPlanning1

        - /MotionPlanning1/Planned Path1

        - /TF1/Frames1

      Splitter Ratio: 0.5

    Tree Height: 379

  - Class: rviz/Help

    Name: Help

  - Class: rviz/Views

    Expanded:

      - /Current View1

    Name: Views

    Splitter Ratio: 0.5

Preferences:

  PromptSaveOnExit: true

Toolbars:

  toolButtonStyle: 2

Visualization Manager:

  Class: ""

  Displays:

    - Alpha: 0.5

      Cell Size: 1

      Class: rviz/Grid

      Color: 160; 160; 164

      Enabled: true

      Line Style:

        Line Width: 0.029999999329447746

        Value: Lines

      Name: Grid

      Normal Cell Count: 0

      Offset:

        X: 0

        Y: 0

        Z: 0

      Plane: XY

      Plane Cell Count: 10

      Reference Frame: <Fixed Frame>

      Value: true

    - Acceleration_Scaling_Factor: 0.1

      Class: moveit_rviz_plugin/MotionPlanning

      Enabled: true

      JointsTab_Use_Radians: true

      Move Group Namespace: ""

      MoveIt_Allow_Approximate_IK: false

      MoveIt_Allow_External_Program: false

      MoveIt_Allow_Replanning: false

      MoveIt_Allow_Sensor_Positioning: false

      MoveIt_Planning_Attempts: 10

      MoveIt_Planning_Time: 5

      MoveIt_Use_Cartesian_Path: false

      MoveIt_Use_Constraint_Aware_IK: false

      MoveIt_Workspace:

        Center:

          X: 0

          Y: 0

          Z: 0

        Size:

          X: 2

          Y: 2

          Z: 2

      Name: MotionPlanning

      Planned Path:

        Color Enabled: false

        Interrupt Display: false

        Links:

          All Links Enabled: true

          Expand Joint Details: false

          Expand Link Details: false

          Expand Tree: false

          Link Tree Style: Links in Alphabetic Order

          base_link:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          gripper_center:

            Alpha: 1

            Show Axes: false

            Show Trail: false

          link1:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          link2:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          link3:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          link4:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          link5:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          link6:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          virtual_roll_link:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          virtual_yaw_link:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          world:

            Alpha: 1

            Show Axes: false

            Show Trail: false

        Loop Animation: false

        Robot Alpha: 0.5

        Robot Color: 150; 50; 150

        Show Robot Collision: false

        Show Robot Visual: false

        Show Trail: false

        State Display Time: 0.05 s

        Trail Step Size: 1

        Trajectory Topic: move_group/display_planned_path

        Use Sim Time: false

      Planning Metrics:

        Payload: 1

        Show Joint Torques: false

        Show Manipulability: false

        Show Manipulability Index: false

        Show Weight Limit: false

        TextHeight: 0.07999999821186066

      Planning Request:

        Colliding Link Color: 255; 0; 0

        Goal State Alpha: 1

        Goal State Color: 250; 128; 0

        Interactive Marker Size: 0.10000000149011612

        Joint Violation Color: 255; 0; 255

        Planning Group: arm_group

        Query Goal State: false

        Query Start State: false

        Show Workspace: false

        Start State Alpha: 1

        Start State Color: 0; 255; 0

      Planning Scene Topic: move_group/monitored_planning_scene

      Robot Description: robot_description

      Scene Geometry:

        Scene Alpha: 1

        Scene Color: 50; 230; 50

        Scene Display Time: 0.009999999776482582

        Show Scene Geometry: true

        Voxel Coloring: Z-Axis

        Voxel Rendering: Occupied Voxels

      Scene Robot:

        Attached Body Color: 150; 50; 150

        Links:

          All Links Enabled: true

          Expand Joint Details: false

          Expand Link Details: false

          Expand Tree: false

          Link Tree Style: Links in Alphabetic Order

          base_link:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          gripper_center:

            Alpha: 1

            Show Axes: false

            Show Trail: false

          link1:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          link2:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          link3:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          link4:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          link5:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          link6:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          virtual_roll_link:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          virtual_yaw_link:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          world:

            Alpha: 1

            Show Axes: false

            Show Trail: false

        Robot Alpha: 0.5

        Show Robot Collision: false

        Show Robot Visual: true

      Value: true

      Velocity_Scaling_Factor: 0.5

    - Class: rviz/TF

      Enabled: true

      Filter (blacklist): ""

      Filter (whitelist): ""

      Frame Timeout: 15

      Frames:

        All Enabled: false

        base_footprint:

          Value: false

        base_link:

          Value: false

        gripper_center:

          Value: true

        link1:

          Value: false

        link2:

          Value: false

        link3:

          Value: false

        link4:

          Value: true

        link5:

          Value: false

        link6:

          Value: false

        virtual_roll_link:

          Value: false

        virtual_yaw_link:

          Value: false

        world:

          Value: true

      Marker Alpha: 1

      Marker Scale: 0.20000000298023224

      Name: TF

      Show Arrows: true

      Show Axes: true

      Show Names: true

      Tree:

        world:

          base_link:

            base_footprint:

              {}

            link1:

              link2:

                link3:

                  link4:

                    link5:

                      {}

                    link6:

                      {}

                    virtual_roll_link:

                      virtual_yaw_link:

                        gripper_center:

                          {}

      Update Interval: 0

      Value: true

  Enabled: true

  Global Options:

    Background Color: 48; 48; 48

    Default Light: true

    Fixed Frame: world

    Frame Rate: 30

  Name: root

  Tools:

    - Class: rviz/Interact

      Hide Inactive Objects: true

    - Class: rviz/MoveCamera

    - Class: rviz/Select

  Value: true

  Views:

    Current:

      Class: rviz/Orbit

      Distance: 1.1154862642288208

      Enable Stereo Rendering:

        Stereo Eye Separation: 0.05999999865889549

        Stereo Focal Distance: 1

        Swap Stereo Eyes: false

        Value: false

      Field of View: 0.75

      Focal Point:

        X: 0.2888287603855133

        Y: 0.12911716103553772

        Z: 0.1591722071170807

      Focal Shape Fixed Size: true

      Focal Shape Size: 0.05000000074505806

      Invert Z Axis: false

      Name: Current View

      Near Clip Distance: 0.009999999776482582

      Pitch: 0.7400000095367432

      Target Frame: world

      Yaw: 0.5767584443092346

    Saved: ~

Window Geometry:

  Displays:

    collapsed: false

  Height: 1016

  Help:

    collapsed: false

  Hide Left Dock: false

  Hide Right Dock: false

  MotionPlanning:

    collapsed: false

  MotionPlanning - Trajectory Slider:

    collapsed: false

  QMainWindow State: 000000ff00000000fd0000000100000000000001f30000039efc0200000007fb000000100044006900730070006c006100790073010000003d000001b8000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001fb000001e00000017d00ffffff000005530000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000

  Views:

    collapsed: false

  Width: 1868

  X: 52

  Y: 27

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/moveit_rviz.launch

0 → 100755
+15 −0
Original line number Original line Diff line number Diff line 
<launch>



  <arg name="debug" default="false" />

  <arg unless="$(arg debug)" name="launch_prefix" value="" />

  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />



  <arg name="rviz_config" default="" />

  <arg     if="$(eval rviz_config=='')" name="command_args" value="" />

  <arg unless="$(eval rviz_config=='')" name="command_args" value="-d $(arg rviz_config)" />



  <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"

        args="$(arg command_args)" output="screen">

  </node>



</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/ompl-chomp_planning_pipeline.launch.xml

0 → 100755
+20 −0
Original line number Original line Diff line number Diff line 
<launch>

  <!-- load OMPL planning pipeline, but add the CHOMP planning adapter. -->

  <include file="$(find arm_robot_moveit)/launch/ompl_planning_pipeline.launch.xml">

    <arg name="planning_adapters"

         default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed

                  default_planner_request_adapters/AddTimeParameterization

                  default_planner_request_adapters/FixWorkspaceBounds

                  default_planner_request_adapters/FixStartStateBounds

                  default_planner_request_adapters/FixStartStateCollision

                  default_planner_request_adapters/FixStartStatePathConstraints

                  chomp/OptimizerAdapter"

                  />

  </include>



  <!-- load chomp config -->

  <rosparam command="load" file="$(find arm_robot_moveit)/config/chomp_planning.yaml" />



  <!-- override trajectory_initialization_method: Use OMPL-generated trajectory -->

  <param name="trajectory_initialization_method" value="fillTrajectory"/>

</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/ompl_planning_pipeline.launch.xml

0 → 100755
+24 −0
Original line number Original line Diff line number Diff line 
<launch>



  <!-- The request adapters (plugins) used when planning with OMPL. ORDER MATTERS! -->

  <arg name="planning_adapters"

       default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed

                default_planner_request_adapters/AddTimeParameterization

                default_planner_request_adapters/ResolveConstraintFrames

                default_planner_request_adapters/FixWorkspaceBounds

                default_planner_request_adapters/FixStartStateBounds

                default_planner_request_adapters/FixStartStateCollision

                default_planner_request_adapters/FixStartStatePathConstraints"

                />



  <arg name="start_state_max_bounds_error" default="0.1" />

  <arg name="jiggle_fraction" default="0.05" />



  <param name="planning_plugin" value="ompl_interface/OMPLPlanner" />

  <param name="request_adapters" value="$(arg planning_adapters)" />

  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />

  <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />



  <rosparam command="load" file="$(find arm_robot_moveit)/config/ompl_planning.yaml"/>



</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml

0 → 100755
+15 −0
Original line number Original line Diff line number Diff line 
<launch>



  <!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->

  <arg name="planning_adapters" default="" />



  <param name="planning_plugin" value="pilz_industrial_motion_planner::CommandPlanner" />

  <param name="request_adapters" value="$(arg planning_adapters)" />



  <!-- Define default planner (for all groups) -->

  <param name="default_planner_config" value="PTP" />



  <!-- MoveGroup capabilities to load for this pipeline, append sequence capability -->

  <param name="capabilities" value="pilz_industrial_motion_planner/MoveGroupSequenceAction

                                    pilz_industrial_motion_planner/MoveGroupSequenceService" />

</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/planning_context.launch

0 → 100755
+26 −0
Original line number Original line Diff line number Diff line 
<launch>

  <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->

  <arg name="load_robot_description" default="false"/>



  <!-- The name of the parameter under which the URDF is loaded -->

  <arg name="robot_description" default="robot_description"/>



  <!-- Load universal robot description format (URDF) -->

  <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find arm_robot)/urdf/arm_robot.urdf"/>



  <!-- The semantic description that corresponds to the URDF -->

  <param name="$(arg robot_description)_semantic" textfile="$(find arm_robot_moveit)/config/arm_robot.srdf" />



  <!-- Load updated joint limits (override information from URDF) -->

  <group ns="$(arg robot_description)_planning">

    <rosparam command="load" file="$(find arm_robot_moveit)/config/joint_limits.yaml"/>

    <rosparam command="load" file="$(find arm_robot_moveit)/config/cartesian_limits.yaml"/>

  </group>



  <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->

  <group ns="$(arg robot_description)_kinematics">

    <rosparam command="load" file="$(find arm_robot_moveit)/config/kinematics.yaml"/>



  </group>



</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/planning_pipeline.launch.xml

0 → 100755
+10 −0
Original line number Original line Diff line number Diff line 
<launch>



  <!-- This file makes it easy to include different planning pipelines;

       It is assumed that all planning pipelines are named XXX_planning_pipeline.launch  -->



  <arg name="pipeline" default="ompl" />



  <include ns="$(arg pipeline)" file="$(dirname)/$(arg pipeline)_planning_pipeline.launch.xml" />



</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/real_arm_robot.launch

0 → 100755
+37 −0
Original line number Original line Diff line number Diff line 
<launch>



  <arg name="use_gui" default="false" />



  <!-- If needed, broadcast static tf for robot root -->

  <node name="tf_footprint_base" pkg="tf" type="static_transform_publisher"

    args="0 0 0 0 0 0 base_link base_footprint 40" />



  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">

    <param name="use_gui" value="$(arg use_gui)" />

    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>

  </node>

  <!-- Robot State Publisher for TF of each joint: publishes all the current states of the joint,

  then RViz can visualize -->

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"

    respawn="false" output="screen" />

  <include file="$(find arm_robot_moveit)/launch/fake_moveit_controller_manager.launch.xml" />

  <!-- Launch Moveit Move Group Node -->

  <include file="$(find arm_robot_moveit)/launch/move_group.launch">

    <arg name="load_robot_description" default="true" />

    <arg name="moveit_controller_manager" value="fake" />

  </include>



  <rosparam file="$(find arm_robot_moveit)/config/ros_controllers.yaml" />

  <!-- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"

    output="screen" args="joint_state_controller arm_group_controller gripper_group_controller" /> -->



  <!-- Run Rviz and load the default configuration to see the state of the move_group node -->

  <arg name="use_rviz" default="true" />



  <include file="$(find arm_robot_moveit)/launch/moveit_rviz.launch" if="$(arg use_rviz)">

    <arg name="rviz_config" value="$(find arm_robot_moveit)/launch/moveit.rviz" />

    <arg name="debug" value="false" />

  </include>

  <!-- <node name="pause_resume_service_node" pkg="arm_robot_moveit" type="pause_resume_service.py"

    output="screen"> </node> -->

</launch>
 
 No newline at end of file

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/ros_control_moveit_controller_manager.launch.xml

0 → 100755
+4 −0
Original line number Original line Diff line number Diff line 
<launch>

	<!-- Define MoveIt controller manager plugin -->

	<param name="moveit_controller_manager" value="moveit_ros_control_interface::MoveItControllerManager" />

</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/ros_controllers.launch

0 → 100755
+11 −0
Original line number Original line Diff line number Diff line 
<?xml version="1.0"?>

<launch>



  <!-- Load joint controller configurations from YAML file to parameter server -->

  <rosparam file="$(find arm_robot_moveit)/config/ros_controllers.yaml" command="load"/>



  <!-- Load the controllers -->

  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"

    output="screen" args=""/>



</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/run_benchmark_ompl.launch

0 → 100755
+21 −0
Original line number Original line Diff line number Diff line 
<launch>



  <!-- This argument must specify the list of .cfg files to process for benchmarking -->

  <arg name="cfg" />



  <!-- Load URDF -->

  <include file="$(dirname)/planning_context.launch">

    <arg name="load_robot_description" value="true"/>

  </include>



  <!-- Start the database -->

  <include file="$(dirname)/warehouse.launch">

    <arg name="moveit_warehouse_database_path" value="moveit_ompl_benchmark_warehouse"/>

  </include>



  <!-- Start Benchmark Executable -->

  <node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">

    <rosparam command="load" file="$(find arm_robot_moveit)/config/ompl_planning.yaml"/>

  </node>



</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/sensor_manager.launch.xml

0 → 100755
+17 −0
Original line number Original line Diff line number Diff line 
<launch>



  <!-- This file makes it easy to include the settings for sensor managers -->



  <!-- Params for 3D sensors config -->

  <rosparam command="load" file="$(find arm_robot_moveit)/config/sensors_3d.yaml" />



  <!-- Params for the octomap monitor -->

  <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->

  <param name="octomap_resolution" type="double" value="0.025" />

  <param name="max_range" type="double" value="5.0" />



  <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->

  <arg name="moveit_sensor_manager" default="arm_robot" />

  <include file="$(dirname)/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />



</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/setup_assistant.launch

0 → 100755
+16 −0
Original line number Original line Diff line number Diff line 
<!-- Re-launch the MoveIt Setup Assistant with this configuration package already loaded -->

<launch>



  <!-- Debug Info -->

  <arg name="debug" default="false" />

  <arg unless="$(arg debug)" name="launch_prefix" value="" />

  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />



  <!-- Run -->

  <node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant"

        args="--config_pkg=arm_robot_moveit"

        launch-prefix="$(arg launch_prefix)"

        required="true"

        output="screen" />



</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/simple_moveit_controller_manager.launch.xml

0 → 100755
+9 −0
Original line number Original line Diff line number Diff line 
<launch>

  <!-- Define the MoveIt controller manager plugin to use for trajectory execution -->

  <param name="moveit_controller_manager"

    value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />



  <!-- Load controller list to the parameter server -->

  <rosparam file="$(find arm_robot_moveit)/config/simple_moveit_controllers.yaml" />

  <!-- <rosparam file="$(find arm_robot_moveit)/config/ros_controllers.yaml" /> -->

</launch>
 
 No newline at end of file

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/stomp_planning_pipeline.launch.xml

0 → 100755
+23 −0
Original line number Original line Diff line number Diff line 
<launch>

  <!-- Stomp Plugin for MoveIt -->

  <arg name="planning_plugin" value="stomp_moveit/StompPlannerManager" />



  <arg name="start_state_max_bounds_error" value="0.1" />

  <arg name="jiggle_fraction" value="0.05" />

  <!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->

  <arg name="planning_adapters"

       default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed

                default_planner_request_adapters/AddTimeParameterization

                default_planner_request_adapters/FixWorkspaceBounds

                default_planner_request_adapters/FixStartStateBounds

                default_planner_request_adapters/FixStartStateCollision

                default_planner_request_adapters/FixStartStatePathConstraints" />





  <param name="planning_plugin" value="$(arg planning_plugin)" />

  <param name="request_adapters" value="$(arg planning_adapters)" />

  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />

  <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />



  <rosparam command="load" file="$(find arm_robot_moveit)/config/stomp_planning.yaml"/>

</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/trajectory_execution.launch.xml

0 → 100755
+23 −0
Original line number Original line Diff line number Diff line 
<launch>

  <!-- This file summarizes all settings required for trajectory execution  -->



  <!-- Define moveit controller manager plugin: fake, simple, or ros_control -->

  <arg name="moveit_controller_manager" />

  <arg name="fake_execution_type" default="interpolate" />



  <!-- Flag indicating whether MoveIt is allowed to load/unload  or switch controllers -->

  <arg name="moveit_manage_controllers" default="true"/>

  <param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>



  <!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->

  <param name="trajectory_execution/allowed_execution_duration_scaling" value="1.2"/> <!-- default 1.2 -->

  <!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->

  <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->

  <!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->

  <param name="trajectory_execution/allowed_start_tolerance" value="0.01"/> <!-- default 0.01 -->



  <!-- We use pass_all_args=true here to pass fake_execution_type, which is required by fake controllers, but not by real-robot controllers.

       As real-robot controller_manager.launch files shouldn't be required to define this argument, we use the trick of passing all args. -->

  <include file="$(dirname)/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" pass_all_args="true" />



</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/warehouse.launch

0 → 100755
+15 −0
Original line number Original line Diff line number Diff line 
<launch>



  <!-- The path to the database must be specified -->

  <arg name="moveit_warehouse_database_path" />



  <!-- Load warehouse parameters -->

  <include file="$(dirname)/warehouse_settings.launch.xml" />



  <!-- Run the DB server -->

  <node name="$(anon mongo_wrapper_ros)" cwd="ROS_HOME" type="mongo_wrapper_ros.py" pkg="warehouse_ros_mongo">

    <param name="overwrite" value="false"/>

    <param name="database_path" value="$(arg moveit_warehouse_database_path)" />

  </node>



</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/warehouse_settings.launch.xml

0 → 100755
+16 −0
Original line number Original line Diff line number Diff line 
<launch>

  <!-- Set the parameters for the warehouse and run the mongodb server. -->



  <!-- The default DB port for moveit (not default MongoDB port to avoid potential conflicts) -->

  <arg name="moveit_warehouse_port" default="33829" />



  <!-- The default DB host for moveit -->

  <arg name="moveit_warehouse_host" default="localhost" />



  <!-- Set parameters for the warehouse -->

  <param name="warehouse_port" value="$(arg moveit_warehouse_port)"/>

  <param name="warehouse_host" value="$(arg moveit_warehouse_host)"/>

  <param name="warehouse_exec" value="mongod" />

  <param name="warehouse_plugin" value="warehouse_ros_mongo::MongoDatabaseConnection" />



</launch>

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/package.xml

0 → 100755
+50 −0
Original line number Original line Diff line number Diff line 
<package>



  <name>arm_robot_moveit</name>

  <version>0.3.0</version>

  <description>

     An automatically generated package with all the configuration and launch files for using the arm_robot with the MoveIt Motion Planning Framework

  </description>

  <author email="abhirup.das@rwth-aachen.de">Abhirup Das</author>

  <maintainer email="abhirup.das@rwth-aachen.de">Abhirup Das</maintainer>



  <license>BSD</license>



  <url type="website">http://moveit.ros.org/</url>

  <url type="bugtracker">https://github.com/moveit/moveit/issues</url>

  <url type="repository">https://github.com/moveit/moveit</url>



  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>roscpp</build_depend>

  <build_depend>rospy</build_depend>

  <build_depend>std_msgs</build_depend>

  <build_depend>std_srvs</build_depend>

  <run_depend>roscpp</run_depend>

  <run_depend>rospy</run_depend>

  <run_depend>std_msgs</run_depend>

  <run_depend>std_srvs</run_depend>





  <run_depend>moveit_ros_move_group</run_depend>

  <run_depend>moveit_fake_controller_manager</run_depend>

  <run_depend>moveit_kinematics</run_depend>

  <run_depend>moveit_planners</run_depend>

  <run_depend>moveit_ros_visualization</run_depend>

  <run_depend>moveit_setup_assistant</run_depend>

  <run_depend>moveit_simple_controller_manager</run_depend>

  <run_depend>joint_state_publisher</run_depend>

  <run_depend>joint_state_publisher_gui</run_depend>

  <run_depend>robot_state_publisher</run_depend>

  <run_depend>rviz</run_depend>

  <run_depend>tf2_ros</run_depend>

  <run_depend>xacro</run_depend>

  <!-- The next 2 packages are required for the gazebo simulation.

       We don't include them by default to prevent installing gazebo and all its dependencies. -->

  <!-- <run_depend>joint_trajectory_controller</run_depend> -->

  <!-- <run_depend>gazebo_ros_control</run_depend> -->

  <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->

  <!-- <run_depend>warehouse_ros_mongo</run_depend> -->

  <run_depend>arm_robot</run_depend>





</package>
 
 No newline at end of file

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/scripts/gettestpose.py

0 → 100755
+46 −0
Original line number Original line Diff line number Diff line 
#!/usr/bin/env python3

import sys

import rospy

import moveit_commander

import tf

from geometry_msgs.msg import Pose



def get_end_effector_pose():

    # Initialize moveit_commander and rospy

    moveit_commander.roscpp_initialize(sys.argv)

    rospy.init_node('get_end_effector_pose', anonymous=True)



    # Initialize the robot, scene, and planning group

    robot = moveit_commander.RobotCommander()

    scene = moveit_commander.PlanningSceneInterface()

    group = moveit_commander.MoveGroupCommander("arm_group")  # Replace "arm" with your robot's planning group



    # Get the current pose of the end effector

    end_effector_pose = group.get_current_pose().pose



    # Extract quaternion from pose

    quaternion = (

        end_effector_pose.orientation.x,

        end_effector_pose.orientation.y,

        end_effector_pose.orientation.z,

        end_effector_pose.orientation.w

    )



    # Convert quaternion to Euler angles (roll, pitch, yaw)

    euler = tf.transformations.euler_from_quaternion(quaternion)



    # Extract roll, pitch, yaw values

    roll = euler[0]

    pitch = euler[1]

    yaw = euler[2]



    # Print out the result

    rospy.loginfo("End Effector Pose: \nPosition: {}\nOrientation (Euler Angles): Roll: {}, Pitch: {}, Yaw: {}".format(

        end_effector_pose.position, roll, pitch, yaw))



    # Close moveit_commander

    moveit_commander.roscpp_shutdown()



if __name__ == "__main__":

    get_end_effector_pose()

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/scripts/pause_resume_service.py

0 → 100755
+34 −0
Original line number Original line Diff line number Diff line 
#! /usr/bin/python3



from std_srvs.srv import SetBool, SetBoolResponse  # Import the correct response

import rospy

from moveit_commander import RobotCommander, PlanningSceneInterface, MoveGroupCommander



# Function to pause the execution

def pause_execution(request):

    move_group.stop()  # Stop the robot's movement

    return SetBoolResponse(success=True, message="Execution Paused")



# Function to resume the execution

def resume_execution(request):

    target_pose = move_group.get_current_pose().pose  # Use current pose or set a new one

    move_group.set_pose_target(target_pose)

    move_group.go(wait=True)  # Resume the robot's movement

    return SetBoolResponse(success=True, message="Execution Resumed")



def main():

    rospy.init_node('pause_resume_execution_service')



    # Initialize the MoveGroupCommander for the robot's planning group (e.g., 'arm')

    global move_group

    move_group = MoveGroupCommander("arm_group")



    # Define the custom services for pausing and resuming execution

    pause_service = rospy.Service('/pause_execution', SetBool, pause_execution)

    resume_service = rospy.Service('/resume_execution', SetBool, resume_execution)



    rospy.loginfo("Services are running...")

    rospy.spin()



if __name__ == "__main__":

    main()

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/scripts/pick_place_fk.py

0 → 100755
+338 −0
Original line number Original line Diff line number Diff line 
#! /usr/bin/python3



import rospy

import sys

import math

import actionlib

import moveit_commander

from moveit_msgs.msg import CollisionObject, ExecuteTrajectoryAction, ExecuteTrajectoryGoal

import geometry_msgs.msg

from shape_msgs.msg import SolidPrimitive

from tf.transformations import euler_from_quaternion, quaternion_from_euler

from std_msgs.msg import String

from moveit_commander.conversions import pose_to_list



class ArmRobot:



	def __init__(self, Group_Name, Grasping_Group_Name):

		'''

		Function to Intialize the Node

		'''

		rospy.loginfo("--- Starting initialising a robotics node ---")

    		

		#initialize moveit Commander

		self._commander = moveit_commander.roscpp_initialize(sys.argv)

		

		#RobotCommander Initialize-Outerlevel Interface to Robot

		self._robot = moveit_commander.RobotCommander()

		

		#instantiate planning scene

		self._scene = moveit_commander.PlanningSceneInterface()

		

		#defining move groups

		self._planning_group = Group_Name

		self._grasping_group = Grasping_Group_Name

		

		#Instantiate MoveGroupCommander Object	

		self._group = moveit_commander.MoveGroupCommander(self._planning_group)

		self._eef_group = moveit_commander.MoveGroupCommander(self._grasping_group)

		

		#Get the planning frame, end effector link and the robot group names

		self._planning_frame = self._group.get_planning_frame()

		self._eef_link = self._group.get_end_effector_link()

		self._group_names = self._robot.get_group_names()

		self._group.set_goal_position_tolerance(1E-2)

		self._group.set_goal_orientation_tolerance(1E-3)

		self._group.set_planning_time(15) #setting planning time in seconds

	

		self._execute_trajectory_client = actionlib.SimpleActionClient('execute_trajectory', ExecuteTrajectoryAction)

		self._execute_trajectory_client.wait_for_server()

		#print the info

		#here the '\033[95m' represents the standard colour "LightMagenta" in terminals. For details, refer: https://pkg.go.dev/github.com/whitedevops/colors

		#The '\033[0m' is added at the end of string to reset the terminal colours to default

		rospy.loginfo('\033[95m' + "Planning Group: {}".format(self._planning_frame) + '\033[0m')

		rospy.loginfo('\033[95m' + "End Effector Link: {}".format(self._eef_link) + '\033[0m')

		rospy.loginfo('\033[95m' + "Group Names: {}".format(self._group_names) + '\033[0m')

		rospy.loginfo('\033[95m' + " >>> ArmRobot initialization is done." + '\033[0m')

		

		

	def wait_for_state_update(self,obj_id,obj_is_known=False, obj_is_attached=False, timeout=2):

		scene = self._scene

		start_time = rospy.get_time()

		secondary_time = rospy.get_time()

				

		while(secondary_time-start_time < timeout) and not rospy.is_shutdown():

			attached_objects = scene.get_attached_objects([obj_id])

			is_attached = len(attached_objects.keys()) > 0

			

			#test

			is_known = obj_id in scene.get_known_object_names()

			

			if (obj_is_attached == is_attached) and (obj_is_known == is_known):

				return True

				

			#sleep to promote other threads

			rospy.sleep(0.1)

			secondary_time = rospy.get_time()

			

		#if while loop exited without returning

		return False

	

	def create_object(self,obj_id,pose,ref_frame, dims, timeout=2):



		object = CollisionObject() #object variable type



		object.id = obj_id

		object.header.frame_id = ref_frame



		solid = SolidPrimitive()

		solid.type = solid.BOX

		solid.dimensions = dims

		object.primitives = [solid]

		scene = self._scene



		## First, create an message object of type pose to deifne box positions

		object_pose = geometry_msgs.msg.Pose()

		#box_pose.header.frame_id=self._armrobot.get_planning_frame()

		#object_pose.header.frame_id = ref_frame



		#set the position massage arguments

		object_pose.position.x = pose[0]

		object_pose.position.y = pose[1]

		object_pose.position.z = pose[2]

		

		#creating quaternion from (rpy)

		orientation_ = quaternion_from_euler(pose[3], pose[4], pose[5])

		

		object_pose.orientation.x = orientation_[0]

		object_pose.orientation.y = orientation_[1]

		object_pose.orientation.x = orientation_[2]

		object_pose.orientation.w = orientation_[3]



		#Adding Object 

		object.primitive_poses = [object_pose] 

		object.operation = object.ADD

		#Add the box in the scene

		scene.add_object(object)

		#https://docs.ros.org/en/noetic/api/moveit_commander/html/classmoveit__commander_1_1planning__scene__interface_1_1PlanningSceneInterface.html

		#https://docs.ros.org/en/noetic/api/moveit_commander/html/planning__scene__interface_8py_source.html

		#Check if the box is added sucessfully

		return self.wait_for_state_update(obj_id, obj_is_known=True, timeout=timeout)

	

	def gripper_action(self,gripper, action="open"):

		'''

		Function to execute the required action of the gripper.



		Args:

			gripper:    group_name of gripper in the manipulator



			action:         action to perform. Allowed values ["open", "close"]



		Returns:

			None.

		'''

		print("Executing gripper action = ", action)



		if(action == "open"):

			self.set_pose(gripper,"gripper_open")

		elif(action == "close"):

			self.set_pose(gripper,"gripper_close")

		else:

			print("Action undefined") 



	def set_pose(self,group,arg_pose_name):

		group.set_named_target(arg_pose_name)

		plan_success, plan, planning_time, error_code = group.plan()

		goal=ExecuteTrajectoryGoal()

		goal.trajectory = plan

		self._execute_trajectory_client.send_goal(goal)

		self._execute_trajectory_client.wait_for_result()

		rospy.loginfo('\033[32m' + "Now at: {}".format(arg_pose_name) + '\033[0m')



	def pick_action(self,scene, robot, arm, gripper,box):

		# Gripper open

		self.gripper_action(gripper, action="open")

		# Sleep

		rospy.sleep(0.5)



		# 1) Move to pre-pick location

		self.set_pose(arm,"pre_pick_pose_1")

		# Sleep

		rospy.sleep(0.5)



		# 2) Move to pick location

		self.set_pose(arm,"pick_pose_1")

		# Sleep

		rospy.sleep(0.5)



		# 3) Close the gripper and attach the object

		# Close gripper

		self.gripper_action(gripper, action="close")

		# Attaching the object

		eef_frame = arm.get_end_effector_link()

		grasping_group = self._grasping_group

		touch_links= robot.get_link_names(group=grasping_group)

		scene.attach_box(eef_frame, box, touch_links=touch_links)

		

		# Sleep

		rospy.sleep(0.5)



		# 4) Move to post-pick location

		# Define post-pick pose

		post_pick_pose = "pre_pick_pose_1"

		# Move

		self.set_pose(arm, post_pick_pose)

		# Sleep

		rospy.sleep(0.5)

		return None        

	

	# Object Place function

	def place_action(self, scene, arm, gripper, box):

		

		# 1) Move to pre-drop location

		# Move

		pre_place_pose = "pre_place_pose"

		self.set_pose(arm, pre_place_pose)

		# Sleep

		rospy.sleep(0.5)



		# 2) Move to drop location

		place_pose = "place_pose"

		self.set_pose(arm, place_pose)

		# Sleep

		rospy.sleep(0.5)



		# 3) Open the gripper and detach the object

		# Open gripper

		self.gripper_action(gripper, action="open")

		# Detaching the object

		eef_frame = arm.get_end_effector_link()

		scene.remove_attached_object(eef_frame, name=box)

		# Sleep

		rospy.sleep(0.5)



		# 4) Move to post-drop location

		# Define post-drop pose

		post_place_pose="pre_place_pose"

		# Move

		self.set_pose(arm, post_place_pose)

		# Sleep

		rospy.sleep(0.5)

		return None



	#==================================================================

	def pick_action_reverse(self,scene, robot, arm, gripper,box):

		# Gripper open

		self.gripper_action(gripper, action="open")

		# Sleep

		rospy.sleep(0.5)



		# 1) Move to pre-pick location

		self.set_pose(arm,"pre_place_pose")

		# Sleep

		rospy.sleep(0.5)



		# 2) Move to pick location

		self.set_pose(arm,"place_pose")

		# Sleep

		rospy.sleep(0.5)



		# 3) Close the gripper and attach the object

		# Close gripper

		self.gripper_action(gripper, action="close")

		# Attaching the object

		eef_frame = arm.get_end_effector_link()

		grasping_group = self._grasping_group

		touch_links= robot.get_link_names(group=grasping_group)

		scene.attach_box(eef_frame, box, touch_links=touch_links)

		

		# Sleep

		rospy.sleep(0.5)



		# 4) Move to post-pick location

		# Define post-pick pose

		post_pick_pose = "pre_place_pose"

		# Move

		self.set_pose(arm, post_pick_pose)

		# Sleep

		rospy.sleep(0.5)

		return None        

	

	# Object Place function

	def place_action_reverse(self, scene, arm, gripper, box):

		

		# 1) Move to pre-drop location

		# Move

		pre_place_pose = "pre_pick_pose_1"

		self.set_pose(arm, pre_place_pose)

		# Sleep

		rospy.sleep(0.5)



		# 2) Move to drop location

		place_pose = "pick_pose_1"

		self.set_pose(arm, place_pose)

		# Sleep

		rospy.sleep(0.5)



		# 3) Open the gripper and detach the object

		# Open gripper

		self.gripper_action(gripper, action="open")

		# Detaching the object

		eef_frame = arm.get_end_effector_link()

		scene.remove_attached_object(eef_frame, name=box)

		# Sleep

		rospy.sleep(0.5)



		# 4) Move to post-drop location

		# Define post-drop pose

		post_place_pose="pre_pick_pose_1"

		# Move

		self.set_pose(arm, post_place_pose)

		# Sleep

		rospy.sleep(0.5)

		return None

	#==================================================================



	# Destructor

	def __del__(self):

		moveit_commander.roscpp_shutdown()

		rospy.loginfo('\033[94m' + "Object of class ArmRobot Deleted." + '\033[0m')



def main():

	#Initialize a ROS Node

	rospy.init_node('add_attach_detach_objects_in_Rviz', anonymous=True)



	robotArm = ArmRobot("arm_group", "gripper_group")

	robotArm._scene.remove_world_object()

	rospy.loginfo("-- Adding Objects --")

	# robotArm.create_object(obj_id="ceiling", ref_frame=robotArm._planning_frame,pose=[0.0,0,0.30,0.0,0.0,0.0],dims=[0.4,0.4,0.01])

	robotArm.create_object(obj_id="table1", ref_frame=robotArm._planning_frame,pose=[0.2,0,-0.01,0.0,0.0,0.0],dims=[0.2,0.5,0.01])

	robotArm.create_object(obj_id="table2", ref_frame=robotArm._planning_frame,pose=[-0.2,0,-0.01,0.0,0.0,0.0],dims=[0.2,0.5,0.01])

	robotArm.create_object(obj_id="box1", ref_frame=robotArm._planning_frame,pose=[0.2635,-0.0275,0.0300,0.0,0.0,0.0],dims=[0.04,0.04,0.04])

	#robotArm.add_box("package$2",0.00,0.6318,0.015,0.03,0.03,0.03)

	rospy.loginfo("Picking object 1")



	#robotArm.pick_action(robotArm._scene, robotArm._robot, robotArm._group, robotArm._eef_group,box="box1")

	# robotArm.set_pose(robotArm._group,"arm_home")

	# Place box1

	#rospy.loginfo("Placing object 1")

	#robotArm.place_action(robotArm._scene, robotArm._group, robotArm._eef_group, box="box1")



	#==================================================================

	for i in range(5):

		robotArm.pick_action(robotArm._scene, robotArm._robot, robotArm._group, robotArm._eef_group,box="box1")

		rospy.loginfo("Placing object 1")

		robotArm.place_action(robotArm._scene, robotArm._group, robotArm._eef_group, box="box1")



		robotArm.pick_action_reverse(robotArm._scene, robotArm._robot, robotArm._group, robotArm._eef_group,box="box1")

		rospy.loginfo("Placing object 1")

		robotArm.place_action_reverse(robotArm._scene, robotArm._group, robotArm._eef_group, box="box1")



	#==================================================================



	robotArm.set_pose(robotArm._group,"arm_home")

	robotArm.set_pose(robotArm._eef_group,"gripper_close")

	robotArm._scene.remove_world_object()

    

if __name__ == '__main__':

    main()

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/scripts/pick_place_ik.py

0 → 100755
+325 −0
Original line number Original line Diff line number Diff line 
#! /usr/bin/python3



import rospy

import sys

import math

import actionlib

import moveit_commander

from moveit_msgs.msg import CollisionObject, ExecuteTrajectoryAction, ExecuteTrajectoryGoal

import geometry_msgs.msg

from shape_msgs.msg import SolidPrimitive

from tf.transformations import euler_from_quaternion, quaternion_from_euler

from std_msgs.msg import String

from moveit_commander.conversions import pose_to_list



class ArmRobot:



	def __init__(self, Group_Name, Grasping_Group_Name):

		'''

		Function to Intialize the Node

		'''

		rospy.loginfo("--- Starting initialising a robotics node ---")

    		

		#initialize moveit Commander

		self._commander = moveit_commander.roscpp_initialize(sys.argv)

		

		#RobotCommander Initialize-Outerlevel Interface to Robot

		self._robot = moveit_commander.RobotCommander()

		

		#instantiate planning scene

		self._scene = moveit_commander.PlanningSceneInterface()

		

		#defining move groups

		self._planning_group = Group_Name

		self._grasping_group = Grasping_Group_Name

		

		#Instantiate MoveGroupCommander Object	

		self._group = moveit_commander.MoveGroupCommander(self._planning_group)

		self._eef_group = moveit_commander.MoveGroupCommander(self._grasping_group)

		

		#Get the planning frame, end effector link and the robot group names

		self._planning_frame = self._group.get_planning_frame()

		self._eef_link = self._group.get_end_effector_link()

		self._group_names = self._robot.get_group_names()

		self._group.set_goal_position_tolerance(1E-2)

		self._group.set_goal_orientation_tolerance(1E-3)

		self._group.set_planning_time(15) #setting planning time in seconds

	

		self._execute_trajectory_client = actionlib.SimpleActionClient('execute_trajectory', ExecuteTrajectoryAction)

		self._execute_trajectory_client.wait_for_server()

		#print the info

		#here the '\033[95m' represents the standard colour "LightMagenta" in terminals. For details, refer: https://pkg.go.dev/github.com/whitedevops/colors

		#The '\033[0m' is added at the end of string to reset the terminal colours to default

		rospy.loginfo('\033[95m' + "Planning Group: {}".format(self._planning_frame) + '\033[0m')

		rospy.loginfo('\033[95m' + "End Effector Link: {}".format(self._eef_link) + '\033[0m')

		rospy.loginfo('\033[95m' + "Group Names: {}".format(self._group_names) + '\033[0m')

		rospy.loginfo('\033[95m' + " >>> ArmRobot initialization is done." + '\033[0m')

		

		

	def wait_for_state_update(self,obj_id,obj_is_known=False, obj_is_attached=False, timeout=2):

		scene = self._scene

		start_time = rospy.get_time()

		secondary_time = rospy.get_time()

				

		while(secondary_time-start_time < timeout) and not rospy.is_shutdown():

			attached_objects = scene.get_attached_objects([obj_id])

			is_attached = len(attached_objects.keys()) > 0

			

			#test

			is_known = obj_id in scene.get_known_object_names()

			

			if (obj_is_attached == is_attached) and (obj_is_known == is_known):

				return True

				

			#sleep to promote other threads

			rospy.sleep(0.1)

			secondary_time = rospy.get_time()

			

		#if while loop exited without returning

		return False

	

	def create_object(self,obj_id,pose,ref_frame, dims, timeout=2):



		object = CollisionObject() #object variable type



		object.id = obj_id

		object.header.frame_id = ref_frame



		solid = SolidPrimitive()

		solid.type = solid.BOX

		solid.dimensions = dims

		object.primitives = [solid]

		scene = self._scene



		## First, create an message object of type pose to deifne box positions

		object_pose = geometry_msgs.msg.Pose()

		#box_pose.header.frame_id=self._armrobot.get_planning_frame()

		#object_pose.header.frame_id = ref_frame



		#set the position massage arguments

		object_pose.position.x = pose[0]

		object_pose.position.y = pose[1]

		object_pose.position.z = pose[2]

		

		#creating quaternion from (rpy)

		orientation_ = quaternion_from_euler(pose[3], pose[4], pose[5])

		

		object_pose.orientation.x = orientation_[0]

		object_pose.orientation.y = orientation_[1]

		object_pose.orientation.x = orientation_[2]

		object_pose.orientation.w = orientation_[3]



		#Adding Object 

		object.primitive_poses = [object_pose] 

		object.operation = object.ADD

		#Add the box in the scene

		scene.add_object(object)

		#https://docs.ros.org/en/noetic/api/moveit_commander/html/classmoveit__commander_1_1planning__scene__interface_1_1PlanningSceneInterface.html

		#https://docs.ros.org/en/noetic/api/moveit_commander/html/planning__scene__interface_8py_source.html

		#Check if the box is added sucessfully

		return self.wait_for_state_update(obj_id, obj_is_known=True, timeout=timeout)

	

	def gripper_action(self,gripper, action="open"):

		'''

		Function to execute the required action of the gripper.



		Args:

			gripper:    group_name of gripper in the manipulator



			action:         action to perform. Allowed values ["open", "close"]



		Returns:

			None.

		'''

		print("Executing gripper action = ", action)



		if(action == "open"):

			self.set_pose(gripper,"gripper_open")

		elif(action == "close"):

			self.set_pose(gripper,"gripper_close")

		else:

			print("Action undefined") 



	def set_pose(self,eef,arg_pose_name):

		eef.set_named_target(arg_pose_name)

		plan_success, plan, planning_time, error_code = eef.plan()

		goal=ExecuteTrajectoryGoal()

		goal.trajectory = plan

		self._execute_trajectory_client.send_goal(goal)

		self._execute_trajectory_client.wait_for_result()

		rospy.loginfo('\033[32m' + "Now at: {}".format(arg_pose_name) + '\033[0m')

		

	def move_to_pose(self,arm, goal_pose, angles):

		'''

		Function to move the robot arm to the requested pose



		Args:

			arm:        robot's arm group that should be moved.



			goal_pose:  requested pose (in the planning frame)



		Returns:

			None

		'''

		#NOTE:

		#The planner will create a plan for the robot to reach

		#the goal pose. 



		rospy.logwarn("planning and moving to location")

		gripper_angle = geometry_msgs.msg.Quaternion()



		#Orientation of the tcp(panda_hand_tcp) w.r.t base frame(panda_link0) 

		quaternion = quaternion_from_euler(angles[0],angles[1],angles[2])



		gripper_angle.x = quaternion[0]

		gripper_angle.y = quaternion[1]

		gripper_angle.z = quaternion[2]

		gripper_angle.w = quaternion[3]



		#defining pre-grasp position

		grasp_pose = geometry_msgs.msg.Pose()

		grasp_pose.position.x = goal_pose.position.x

		grasp_pose.position.y = goal_pose.position.y

		grasp_pose.position.z = goal_pose.position.z   

		grasp_pose.orientation = gripper_angle



		print("Exectuting move_to_pose ({} , {},  {})"

				.format(grasp_pose.position.x, grasp_pose.position.y, grasp_pose.position.z))



		arm.set_goal_position_tolerance(0.01) #setting tolerence for pose 

		arm.set_goal_orientation_tolerance(0.1)

		arm.set_pose_target(grasp_pose) #setting the pose of the end-effector



		plan = arm.go(wait=True) #move the arm to the grasp_pose.



		arm.stop() #ensures that there is no residual movement.

		arm.clear_pose_targets() #to clear the existing targets.



	def pick_action(self,scene, robot, arm, gripper, pick_pose, box):

		# Gripper open

		self.gripper_action(gripper, action="open")

		# Sleep

		rospy.sleep(0.5)



		# 1) Move to pre-pick location

		pre_pick_angle = [-1.5716,0.0, -0.1044]

		#Sub 0.036 to x, add 0.004 to y and 0.170 to z for prepick

		# Define pre-pick pose

		pre_pick_pose = geometry_msgs.msg.Pose()

		pre_pick_pose.position.x = pick_pose.position.x - 0.036

		pre_pick_pose.position.y = pick_pose.position.y + 0.004

		pre_pick_pose.position.z = pick_pose.position.z + 0.2

		

		# Move

		pick_angle = [-1.5716,0.7727, -0.1050] 

		self.move_to_pose(arm, pre_pick_pose, pre_pick_angle)

		# Sleep

		rospy.sleep(0.5)



		# 2) Move to pick location

		self.move_to_pose(arm, pick_pose,pick_angle)

		# Sleep

		rospy.sleep(0.5)



		# 3) Close the gripper and attach the object

		# Close gripper

		self.gripper_action(gripper, action="close")

		# Attaching the object

		eef_frame = arm.get_end_effector_link()

		grasping_group = self._grasping_group

		touch_links= robot.get_link_names(group=grasping_group)

		scene.attach_box(eef_frame, box, touch_links=touch_links)

		

		# Sleep

		rospy.sleep(0.5)



		# 4) Move to post-pick location

		# Define post-pick pose

		post_pick_pose = pre_pick_pose

		post_pick_angle = pre_pick_angle

		# Move

		self.move_to_pose(arm, post_pick_pose, post_pick_angle)

		# Sleep

		rospy.sleep(0.5)

		return None        

	

	# Object Place function

	def place_action(self, scene, arm, gripper, place_pose, box):

		

		# 1) Move to pre-drop location

		# Define pre-drop pose

		pre_place_angle = [-1.5611, 0.0784,3.0379]

		pre_place_pose = geometry_msgs.msg.Pose()

		pre_place_pose.position.x = place_pose.position.x + 0.0363

		pre_place_pose.position.y = place_pose.position.y - 0.0044

		pre_place_pose.position.z = place_pose.position.z + 0.2

		# Move

		self.move_to_pose(arm, pre_place_pose, pre_place_angle)

		# Sleep

		rospy.sleep(0.5)



		# 2) Move to drop location

		place_angle = [-1.5716,0.7727, -0.1050]

		self.move_to_pose(arm, place_pose,place_angle)

		# Sleep

		rospy.sleep(0.5)



		# 3) Open the gripper and detach the object

		# Open gripper

		self.gripper_action(gripper, action="open")

		# Detaching the object

		eef_frame = arm.get_end_effector_link()

		scene.remove_attached_object(eef_frame, name=box)

		# Sleep

		rospy.sleep(0.5)



		# 4) Move to post-drop location

		# Define post-drop pose

		post_place_pose=pre_place_pose

		post_place_angle=pre_place_angle

		# Move

		self.move_to_pose(arm, post_place_pose,post_place_angle)

		

		# Sleep

		rospy.sleep(0.5)

		return None



	# Destructor

	def __del__(self):

		moveit_commander.roscpp_shutdown()

		rospy.loginfo('\033[94m' + "Object of class ArmRobot Deleted." + '\033[0m')



def main():

	#Initialize a ROS Node

	rospy.init_node('add_attach_detach_objects_in_Rviz', anonymous=True)



	robotArm = ArmRobot("arm_group", "gripper_group")

	robotArm._scene.remove_world_object()

	rospy.loginfo("-- Adding Objects --")

	robotArm.create_object(obj_id="ceiling", ref_frame=robotArm._planning_frame,pose=[0.0,0,0.30,0.0,0.0,0.0],dims=[0.4,0.4,0.01])

	# robotArm.create_object(obj_id="table1", ref_frame=robotArm._planning_frame,pose=[0.2,0,-0.01,0.0,0.0,0.0],dims=[0.2,0.5,0.01])

	# robotArm.create_object(obj_id="table2", ref_frame=robotArm._planning_frame,pose=[-0.2,0,-0.01,0.0,0.0,0.0],dims=[0.2,0.5,0.01])

	robotArm.create_object(obj_id="box1", ref_frame=robotArm._planning_frame,pose=[0.2635,-0.0275,0.0300,0.0,0.0,0.0],dims=[0.04,0.04,0.04])

	#robotArm.add_box("package$2",0.00,0.6318,0.015,0.03,0.03,0.03)

	rospy.loginfo("Picking object 1")

	pick_pose_1 = geometry_msgs.msg.Pose()

	pick_pose_1.position.x = 0.2635

	pick_pose_1.position.y = -0.0275

	pick_pose_1.position.z = 0.0300

	# pick_pose_1=robotArm._scene.getObject("box1").primitive_poses[0]

	# rospy.loginfo(fpick_pose_1.position.x, pick_pose_1.position.y, pick_pose_1.position.z)

	robotArm.pick_action(robotArm._scene, robotArm._robot, robotArm._group, robotArm._eef_group, pick_pose_1, box="box1")

	robotArm.set_pose(robotArm._group,"arm_home")

	# Place box1

	rospy.loginfo("Placing object 1")

	place_pose_1 = geometry_msgs.msg.Pose()

	place_pose_1.position.x = -0.2635

	place_pose_1.position.y = 0.0275

	place_pose_1.position.z = 0.0300

	robotArm.place_action(robotArm._scene, robotArm._group, robotArm._eef_group, place_pose_1, box="box1")

	robotArm.set_pose(robotArm._group,"arm_home")

	robotArm.set_pose(robotArm._eef_group,"gripper_close")

	robotArm._scene.remove_world_object()

    

if __name__ == '__main__':

    main()

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/scripts/pick_place_ik_copy.py

0 → 100755
+406 −0
Original line number Original line Diff line number Diff line 
#! /usr/bin/python3



import rospy

import sys

import math

import actionlib

import moveit_commander

from moveit_msgs.msg import CollisionObject, ExecuteTrajectoryAction, ExecuteTrajectoryGoal

import geometry_msgs.msg

from shape_msgs.msg import SolidPrimitive

from tf.transformations import euler_from_quaternion, quaternion_from_euler

from std_msgs.msg import String

from moveit_commander.conversions import pose_to_list

import trac_ik_python.trac_ik as tracik



class ArmRobot:



	def __init__(self, Group_Name, Grasping_Group_Name):

		'''

		Function to Intialize the Node

		'''

		rospy.loginfo("--- Starting initialising a robotics node ---")

    		

		#initialize moveit Commander

		self._commander = moveit_commander.roscpp_initialize(sys.argv)

		

		#RobotCommander Initialize-Outerlevel Interface to Robot

		self._robot = moveit_commander.RobotCommander()

		

		#instantiate planning scene

		self._scene = moveit_commander.PlanningSceneInterface()

		

		#defining move groups

		self._planning_group = Group_Name

		self._grasping_group = Grasping_Group_Name

		

		#Instantiate MoveGroupCommander Object	

		self._group = moveit_commander.MoveGroupCommander(self._planning_group)

		self._eef_group = moveit_commander.MoveGroupCommander(self._grasping_group)

		

		#Get the planning frame, end effector link and the robot group names

		self._planning_frame = self._group.get_planning_frame()

		self._eef_link = self._group.get_end_effector_link()

		self._group_names = self._robot.get_group_names()

		self._group.set_goal_position_tolerance(1E-2)

		self._group.set_goal_orientation_tolerance(1E-3)

		self._group.set_planning_time(15) #setting planning time in seconds

	

		self._execute_trajectory_client = actionlib.SimpleActionClient('execute_trajectory', ExecuteTrajectoryAction)

		self._execute_trajectory_client.wait_for_server()

		#print the info

		#here the '\033[95m' represents the standard colour "LightMagenta" in terminals. For details, refer: https://pkg.go.dev/github.com/whitedevops/colors

		#The '\033[0m' is added at the end of string to reset the terminal colours to default

		rospy.loginfo('\033[95m' + "Planning Group: {}".format(self._planning_frame) + '\033[0m')

		rospy.loginfo('\033[95m' + "End Effector Link: {}".format(self._eef_link) + '\033[0m')

		rospy.loginfo('\033[95m' + "Group Names: {}".format(self._group_names) + '\033[0m')

		rospy.loginfo('\033[95m' + " >>> ArmRobot initialization is done." + '\033[0m')

		

		

	def wait_for_state_update(self,obj_id,obj_is_known=False, obj_is_attached=False, timeout=4):

		scene = self._scene

		start_time = rospy.get_time()

		secondary_time = rospy.get_time()

				

		while(secondary_time-start_time < timeout) and not rospy.is_shutdown():

			attached_objects = scene.get_attached_objects([obj_id])

			is_attached = len(attached_objects.keys()) > 0

			

			#test

			is_known = obj_id in scene.get_known_object_names()

			

			if (obj_is_attached == is_attached) and (obj_is_known == is_known):

				return True

				

			#sleep to promote other threads

			rospy.sleep(0.1)

			secondary_time = rospy.get_time()

			

		#if while loop exited without returning

		return False

	

	def create_object(self,obj_id,pose,ref_frame, dims, timeout=4):



		object = CollisionObject() #object variable type



		object.id = obj_id

		object.header.frame_id = ref_frame



		solid = SolidPrimitive()

		solid.type = solid.BOX

		solid.dimensions = dims

		object.primitives = [solid]

		scene = self._scene



		## First, create an message object of type pose to deifne box positions

		object_pose = geometry_msgs.msg.Pose()

		#box_pose.header.frame_id=self._armrobot.get_planning_frame()

		#object_pose.header.frame_id = ref_frame



		#set the position massage arguments

		object_pose.position.x = pose[0]

		object_pose.position.y = pose[1]

		object_pose.position.z = pose[2]

		

		#creating quaternion from (rpy)

		orientation_ = quaternion_from_euler(pose[3], pose[4], pose[5])

		

		object_pose.orientation.x = orientation_[0]

		object_pose.orientation.y = orientation_[1]

		object_pose.orientation.x = orientation_[2]

		object_pose.orientation.w = orientation_[3]



		#Adding Object 

		object.primitive_poses = [object_pose] 

		object.operation = object.ADD

		#Add the box in the scene

		scene.add_object(object)

		#https://docs.ros.org/en/noetic/api/moveit_commander/html/classmoveit__commander_1_1planning__scene__interface_1_1PlanningSceneInterface.html

		#https://docs.ros.org/en/noetic/api/moveit_commander/html/planning__scene__interface_8py_source.html

		#Check if the box is added sucessfully

		return self.wait_for_state_update(obj_id, obj_is_known=True, timeout=timeout)

	

	def gripper_action(self,gripper, action="open"):

		'''

		Function to execute the required action of the gripper.



		Args:

			gripper:    group_name of gripper in the manipulator



			action:         action to perform. Allowed values ["open", "close"]



		Returns:

			None.

		'''

		print("Executing gripper action = ", action)



		if(action == "open"):

			self.set_pose(gripper,"gripper_open")

		elif(action == "close"):

			self.set_pose(gripper,"gripper_close")

		else:

			print("Action undefined") 



	def set_pose(self,eef,arg_pose_name):

		eef.set_named_target(arg_pose_name)

		plan_success, plan, planning_time, error_code = eef.plan()

		goal=ExecuteTrajectoryGoal()

		goal.trajectory = plan

		self._execute_trajectory_client.send_goal(goal)

		self._execute_trajectory_client.wait_for_result()

		rospy.loginfo('\033[32m' + "Now at: {}".format(arg_pose_name) + '\033[0m')

	

	

	def move_to_pose_or_joint(self, arm, goal_pose=None, joint_angles=None, angles=None):

		'''

		Function to move the robot arm either to a requested pose (Cartesian) or joint positions (joint-space)

		

		Args:

			arm:            the robot arm group that should be moved.

			goal_pose:      requested pose (in the planning frame) (optional)

			joint_angles:   joint angles (list) to move to (optional)

			angles:         orientation angles for pose (optional)



		Returns:

			None

		'''

		if joint_angles:  # If joint_angles are provided, move to joint target

			arm.set_joint_value_target(joint_angles)

			success = arm.go(wait=True)



			if success:

				rospy.loginfo("Move to approximate joint angles successful.")

				arm.stop() #ensures that there is no residual movement.

				arm.clear_pose_targets()

			else:

				rospy.logwarn("Move to approximate joint angles failed.")

		elif goal_pose:  # Otherwise, move to pose (Cartesian)

			rospy.logwarn("Planning and moving to location (Cartesian)")



			# Convert Euler angles to quaternion

			quaternion = quaternion_from_euler(angles[0], angles[1], angles[2])

			gripper_angle = geometry_msgs.msg.Quaternion()

			gripper_angle.x = quaternion[0]

			gripper_angle.y = quaternion[1]

			gripper_angle.z = quaternion[2]

			gripper_angle.w = quaternion[3]



			# Define the target pose

			grasp_pose = geometry_msgs.msg.Pose()

			grasp_pose.position.x = goal_pose.position.x

			grasp_pose.position.y = goal_pose.position.y

			grasp_pose.position.z = goal_pose.position.z

			grasp_pose.orientation = gripper_angle



			rospy.loginfo(f"Executing move_to_pose with position: {grasp_pose.position.x}, {grasp_pose.position.y}, {grasp_pose.position.z}")



			ik_solver = tracik.TrackerIK("base_link", "link4", "trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin")





			# Use inverse kinematics to find the joint angles for the target pose

			ik_successful, joint_angles = ik_solver.get_ik(grasp_pose)



			

			if ik_successful:

				rospy.loginfo(f"IK found joint angles: {joint_angles}")

				arm.set_joint_value_target(joint_angles)

				success = arm.go(wait=True)

				if success:

					rospy.loginfo("Move to approximate joint angles successful.")

					arm.stop() #ensures that there is no residual movement.

					arm.clear_pose_targets()

				else:

					rospy.logwarn("Move to approximate joint angles failed.")

			else:

				rospy.logwarn("Inverse Kinematics failed to find a solution.")







	def move_to_pose(self,arm, goal_pose, angles):

		'''

		Function to move the robot arm to the requested pose



		Args:

			arm:        robot's arm group that should be moved.



			goal_pose:  requested pose (in the planning frame)



		Returns:

			None

		'''

		#NOTE:

		#The planner will create a plan for the robot to reach

		#the goal pose. 



		rospy.logwarn("planning and moving to location")

		gripper_angle = geometry_msgs.msg.Quaternion()



		#Orientation of the tcp(panda_hand_tcp) w.r.t base frame(panda_link0) 

		quaternion = quaternion_from_euler(angles[0],angles[1],angles[2])



		gripper_angle.x = quaternion[0]

		gripper_angle.y = quaternion[1]

		gripper_angle.z = quaternion[2]

		gripper_angle.w = quaternion[3]



		#defining pre-grasp position

		grasp_pose = geometry_msgs.msg.Pose()

		grasp_pose.position.x = goal_pose.position.x

		grasp_pose.position.y = goal_pose.position.y

		grasp_pose.position.z = goal_pose.position.z   

		grasp_pose.orientation = gripper_angle



		print("Exectuting move_to_pose ({} , {},  {})"

				.format(grasp_pose.position.x, grasp_pose.position.y, grasp_pose.position.z))



		arm.set_goal_position_tolerance(0.01) #setting tolerence for pose 

		arm.set_goal_orientation_tolerance(0.1)

		arm.set_pose_target(grasp_pose) #setting the pose of the end-effector



		plan = arm.go(wait=True) #move the arm to the grasp_pose.



		arm.stop() #ensures that there is no residual movement.

		arm.clear_pose_targets() #to clear the existing targets.



	def pick_action(self,scene, robot, arm, gripper, pick_pose, box):

		# Gripper open

		self.gripper_action(gripper, action="open")

		# Sleep

		rospy.sleep(1)



		# 1) Move to pre-pick location

		pre_pick_angle = [-1.5716,0.0, -0.1044]

		#Sub 0.036 to x, add 0.004 to y and 0.170 to z for prepick

		# Define pre-pick pose

		pre_pick_pose = geometry_msgs.msg.Pose()

		pre_pick_pose.position.x = pick_pose.position.x - 0.036

		pre_pick_pose.position.y = pick_pose.position.y + 0.004

		pre_pick_pose.position.z = pick_pose.position.z + 0.2

		

		# Move

		self.move_to_pose_or_joint(arm, pre_pick_pose, angles=pre_pick_angle)

		# Sleep

		rospy.sleep(1)

		

		# 2) Move to pick location

		pick_angle = [-1.5716,0.7727, -0.1050] 

		self.move_to_pose_or_joint(arm, pick_pose,angles=pick_angle)

		# Sleep

		rospy.sleep(1)



		# 3) Close the gripper and attach the object

		# Close gripper

		self.gripper_action(gripper, action="close")

		# Attaching the object

		eef_frame = arm.get_end_effector_link()

		grasping_group = self._grasping_group

		touch_links= robot.get_link_names(group=grasping_group)

		scene.attach_box(eef_frame, box, touch_links=touch_links)

		

		# Sleep

		rospy.sleep(1)



		# 4) Move to post-pick location

		# Define post-pick pose

		post_pick_pose = pre_pick_pose

		post_pick_angle = pre_pick_angle

		# Move

		self.move_to_pose_or_joint(arm, post_pick_pose, angles=post_pick_angle)

		# Sleep

		rospy.sleep(1)

		return None        

	

	# Object Place function

	def place_action(self, scene, arm, gripper, place_pose, box):



		# #INTERMEDIATE

		target_pose = geometry_msgs.msg.Pose()

		target_pose.position.x = 0.0248  # Example x position 

		target_pose.position.y = 0.2273  # Example y position

		target_pose.position.z = 0.2330  # Example z position 

		target_angle=[-1.5710, -0.0002, 1.4621]

		self.move_to_pose_or_joint(arm, target_pose,angles=target_angle)

		

		# 1) Move to pre-drop location

		# Define pre-drop pose

		pre_place_angle = [-1.5611, 0.0784,3.0379]

		pre_place_pose = geometry_msgs.msg.Pose()

		pre_place_pose.position.x = place_pose.position.x + 0.0363

		pre_place_pose.position.y = place_pose.position.y - 0.0044

		pre_place_pose.position.z = place_pose.position.z + 0.2

		# Move

		self.move_to_pose_or_joint(arm, pre_place_pose, angles=pre_place_angle)

		# Sleep

		rospy.sleep(1)



		# 2) Move to drop location

		place_angle = [-1.5716,0.7727, -0.1050]

		self.move_to_pose_or_joint(arm, place_pose,angles=place_angle)

		# Sleep

		rospy.sleep(1)



		# 3) Open the gripper and detach the object

		# Open gripper

		self.gripper_action(gripper, action="open")

		# Detaching the object

		eef_frame = arm.get_end_effector_link()

		scene.remove_attached_object(eef_frame, name=box)

		# Sleep

		rospy.sleep(1)



		# 4) Move to post-drop location

		# Define post-drop pose

		post_place_pose=pre_place_pose

		post_place_angle=pre_place_angle

		# Move

		self.move_to_pose_or_joint(arm, post_place_pose,angles=post_place_angle)

		

		# #INTERMEDIATE

		target_pose = geometry_msgs.msg.Pose()

		target_pose.position.x = 0.0248  # Example x position 

		target_pose.position.y = 0.2273  # Example y position

		target_pose.position.z = 0.2330  # Example z position 

		target_angle=[-1.5710, -0.0002, 1.4621]

		self.move_to_pose_or_joint(arm, target_pose,angles=target_angle)

		self.gripper_action(gripper, action="close")

		# Sleep

		rospy.sleep(1)

		return None



	# Destructor

	def __del__(self):

		moveit_commander.roscpp_shutdown()

		rospy.loginfo('\033[94m' + "Object of class ArmRobot Deleted." + '\033[0m')



def main():

	#Initialize a ROS Node

	rospy.init_node('add_attach_detach_objects_in_Rviz', anonymous=True)



	robotArm = ArmRobot("arm_group", "gripper_group")

	robotArm._scene.remove_world_object()

	rospy.loginfo("-- Adding Objects --")

	robotArm.create_object(obj_id="ceiling", ref_frame=robotArm._planning_frame,pose=[0.0,0,0.30,0.0,0.0,0.0],dims=[0.4,0.4,0.01])

	# robotArm.create_object(obj_id="table1", ref_frame=robotArm._planning_frame,pose=[0.2,0,-0.01,0.0,0.0,0.0],dims=[0.2,0.5,0.01])

	# robotArm.create_object(obj_id="table2", ref_frame=robotArm._planning_frame,pose=[-0.2,0,-0.01,0.0,0.0,0.0],dims=[0.2,0.5,0.01])

	robotArm.create_object(obj_id="box1", ref_frame=robotArm._planning_frame,pose=[0.2635,-0.0275,0.0300,0.0,0.0,0.0],dims=[0.04,0.04,0.04])

	#robotArm.add_box("package$2",0.00,0.6318,0.015,0.03,0.03,0.03)

	rospy.loginfo("Picking object 1")

	pick_pose_1 = geometry_msgs.msg.Pose()

	pick_pose_1.position.x = 0.2635

	pick_pose_1.position.y = -0.0275

	pick_pose_1.position.z = 0.0300

	# pick_pose_1=robotArm._scene.getObject("box1").primitive_poses[0]

	# rospy.loginfo(fpick_pose_1.position.x, pick_pose_1.position.y, pick_pose_1.position.z)

	robotArm.pick_action(robotArm._scene, robotArm._robot, robotArm._group, robotArm._eef_group, pick_pose_1, box="box1")

	# Place box1

	rospy.loginfo("Placing object 1")

	place_pose_1 = geometry_msgs.msg.Pose()

	place_pose_1.position.x = -0.2635

	place_pose_1.position.y = 0.0275

	place_pose_1.position.z = 0.0300

	robotArm.place_action(robotArm._scene, robotArm._group, robotArm._eef_group, place_pose_1, box="box1")

	robotArm.set_pose(robotArm._group,"arm_home")

	robotArm._scene.remove_world_object()

    

if __name__ == '__main__':

    main()

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/scripts/sync_plan.py

0 → 100755
+74 −0
Original line number Original line Diff line number Diff line 
#!/usr/bin/env python3

# encoding=utf-8

import time

import math

import rospy

from sensor_msgs.msg import JointState

from adafruit_servokit import ServoKit





kit = None





def callback(data):

    global kit

    # rospy.loginfo(rospy.get_caller_id() + "%s", data)

    data_list = []

    for index, value in enumerate(data.position):

        radians_to_angles = round(math.degrees(value), 2)

        if radians_to_angles>180:

            radians_to_angles = 180

        if index==1:

            radians_to_angles = 90 - radians_to_angles     

        if index>1 and index<4 :

            radians_to_angles = 90 + radians_to_angles

        if index==5:

            radians_to_angles = 180 + radians_to_angles  

        

        data_list.append(round(radians_to_angles,1))

        

    rospy.loginfo(rospy.get_caller_id() + "%s", data_list)

    # mc.send_angles(data_list, 25)

    kit.servo[0].angle = data_list[0]

    kit.servo[1].angle = data_list[1]

    kit.servo[2].angle = data_list[2]

    kit.servo[3].angle = data_list[3]

    kit.servo[4].angle = data_list[4]

    kit.servo[5].angle = data_list[5]





def listener():

    rospy.init_node("motorcontrolNode", anonymous=True)

    time.sleep(0.05)

    

    rospy.Subscriber("joint_states", JointState, callback)



    # spin() simply keeps python from exiting until this node is stopped

    rospy.spin()





if __name__ == "__main__":

    kit = ServoKit(channels=16)

    m = 0

    kit.servo[0].actuation_range = 180

    kit.servo[0].set_pulse_width_range(600, 2600)

    kit.servo[1].actuation_range = 180

    kit.servo[1].set_pulse_width_range(600, 2600)

    kit.servo[2].actuation_range = 180

    kit.servo[2].set_pulse_width_range(600, 2600)

    kit.servo[3].actuation_range = 180

    kit.servo[3].set_pulse_width_range(600, 2600)

    kit.servo[4].actuation_range = 180

    kit.servo[4].set_pulse_width_range(600, 2600)

    kit.servo[5].actuation_range = 180

    kit.servo[5].set_pulse_width_range(600, 2600)

    kit.servo[6].actuation_range = 180

    kit.servo[6].set_pulse_width_range(600, 2600)

   

    kit.servo[0].angle = 90.0

    kit.servo[1].angle = 135.0

    kit.servo[2].angle = 180.0

    kit.servo[3].angle = 135.0

    kit.servo[4].angle = 15.0 

    kit.servo[5].angle = 165.0

    listener()

Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/scripts/testmoveit.py

0 → 100755
+87 −0
Original line number Original line Diff line number Diff line 
#! /usr/bin/python3



import rospy

import sys

import math

import actionlib

import moveit_commander

import geometry_msgs.msg

from moveit_commander import MoveGroupCommander

from tf.transformations import quaternion_from_euler



# Initialize the move group and the robot's planning interface

move_group = MoveGroupCommander("arm_group")  # Replace "arm" with your move group name

move_group.set_planning_time(12)





# # PREPICK

# target_pose = geometry_msgs.msg.Pose()

# target_pose.position.x =  0.2235# Example x position 

# target_pose.position.y = -0.0233 # Example y position

# target_pose.position.z = 0.2330 # Example z position

# #angle = 45 # 45 degrees

# # angle = [-90.12,53.82,-6.046]

# #radians = angle * 3.14159 / 180.0

# # quaternion = quaternion_from_euler(angle[0]* 3.14159 / 180.0, angle[1]* 3.14159 / 180.0 ,angle[2]* 3.14159 / 180.0 )

# quaternion = quaternion_from_euler(-1.5716,0.0780, -0.1044)

# target_pose.orientation.x = quaternion[0]

# target_pose.orientation.y = quaternion[1]

# target_pose.orientation.z = quaternion[2]

# target_pose.orientation.w = quaternion[3]





# # PICK

# target_pose = geometry_msgs.msg.Pose()

# target_pose.position.x = 0.2635 # Example x position 

# target_pose.position.y = -0.0275 # Example y position

# target_pose.position.z = 0.0300 # Example z position

# #radians = angle * 3.14159 / 180.0  

# quaternion = quaternion_from_euler(-1.5716,0.7727, -0.1050)

# target_pose.orientation.x = quaternion[0]

# target_pose.orientation.y = quaternion[1]

# target_pose.orientation.z = quaternion[2]

# target_pose.orientation.w = quaternion[3]



# #INTERMEDIATE

# target_pose = geometry_msgs.msg.Pose()

# target_pose.position.x = 0.0248  # Example x position 

# target_pose.position.y = 0.2273  # Example y position

# target_pose.position.z = 0.2330  # Example z position

# #angle = 45 # 45 degrees

# #radians = angle * 3.14159 / 180.0  

# quaternion = quaternion_from_euler(-1.5710, -0.0002, 1.4621)

# target_pose.orientation.x = quaternion[0]

# target_pose.orientation.y = quaternion[1]

# target_pose.orientation.z = quaternion[2]

# target_pose.orientation.w = quaternion[3]



# PRE PLACE

target_pose = geometry_msgs.msg.Pose()

target_pose.position.x = -0.2256  # Example x position 

target_pose.position.y = 0.0275  # Example y position

target_pose.position.z = 0.2310  # Example z position

#angle = 45 # 45 degrees

#radians = angle * 3.14159 / 180.0  

quaternion = quaternion_from_euler(-1.5434, 0.0167, 3.0377)

target_pose.orientation.x = quaternion[0]

target_pose.orientation.y = quaternion[1]

target_pose.orientation.z = quaternion[2]

target_pose.orientation.w = quaternion[3]



# #PLACE

# target_pose = geometry_msgs.msg.Pose()

# target_pose.position.x = -0.2635 # Example x position 

# target_pose.position.y = 0.0275 # Example y position

# target_pose.position.z = 0.0300 # Example z position

# #radians = angle * 3.14159 / 180.0  

# quaternion = quaternion_from_euler(-1.5716,0.7727,-0.1050)

# target_pose.orientation.x = quaternion[0]

# target_pose.orientation.y = quaternion[1]

# target_pose.orientation.z = quaternion[2]

# target_pose.orientation.w = quaternion[3]



# Set the target pose

move_group.set_pose_target(target_pose)



# Plan and execute the motion

plan = move_group.go(wait=True)

Exercises/catkin_ws/src/pick_and_place/scripts/pick_place_example.py

100644 → 100755
+58 −36
Original line number Original line Diff line number Diff line
@@ -12,6 +12,7 @@ from geometry_msgs.msg import Pose 
from shape_msgs.msg import SolidPrimitive

from shape_msgs.msg import SolidPrimitive

from moveit_msgs.msg import *

from moveit_msgs.msg import *

import tf 

import tf 

import actionlib





def create_object(obj_id, ref_frame, dimensions, pose):

def create_object(obj_id, ref_frame, dimensions, pose):

    '''

    '''
@@ -62,6 +63,7 @@ def create_object(obj_id, ref_frame, dimensions, pose): 
    #Adding Object 

    #Adding Object 

    object.primitive_poses = [object_pose] 

    object.primitive_poses = [object_pose] 

    object.operation = object.ADD

    object.operation = object.ADD

    print(f"Object {obj_id} created with pose: {object_pose.position.x}, {object_pose.position.y}, {object_pose.position.z}")

    return object

    return object





def add_items(scene, frame):

def add_items(scene, frame):
@@ -76,20 +78,20 @@ def add_items(scene, frame): 
        Returns the items that can be picked as an array.

        Returns the items that can be picked as an array.

    '''

    '''

    rospy.loginfo("-- Adding Objects --")

    rospy.loginfo("-- Adding Objects --")

    height_from_ground = 0.3 

    height_from_ground = 0.0 #change this to move the table up or down





    #Define the objects

    #Define the objects

    table1 = create_object(obj_id="table1", ref_frame=frame, 

    table1 = create_object(obj_id="table1", ref_frame=frame, 

                            dimensions=[0.2, 0.5, 0.01], 

                            dimensions=[0.1, 0.5, 0.01], 

                            pose=[0.5, 0, height_from_ground, 0.0, 0.0, 0.0])

                            pose=[0.2, 0.0, height_from_ground, 0.0, 0.0, 0.0],)





    table2 = create_object(obj_id="table2", ref_frame=frame, 

    table2 = create_object(obj_id="table2", ref_frame=frame, 

                            dimensions=[0.2, 0.5, 0.01], 

                            dimensions=[0.1, 0.5, 0.01], 

                            pose=[-0.5, 0, height_from_ground, 0.0, 0.0, 0.0])

                            pose=[-0.2, 0.0, height_from_ground, 0.0, 0.0, 0.0])





    box1 = create_object(obj_id="box1", ref_frame=frame, 

    box1 = create_object(obj_id="box1", ref_frame=frame, 

                         dimensions=[0.1, 0.025, 0.025], 

                         dimensions=[0.05, 0.025, 0.025], 

                         pose=[0.5, 0, height_from_ground+(0.05/2.0), 0.0, 0.0, 0.0])

                         pose=[0.2, 0, height_from_ground+(0.05/2.0), 0.0, 0.0, 0.0])





    #Add Objects to the scene

    #Add Objects to the scene

    scene.add_object(table1)

    scene.add_object(table1)
@@ -97,6 +99,28 @@ def add_items(scene, frame): 
    scene.add_object(box1)

    scene.add_object(box1)

    rospy.sleep(2) #Waiting to spawn the objects in the world

    rospy.sleep(2) #Waiting to spawn the objects in the world





def set_gripper_position(gripper, gripper_action):

    '''

        Function to set named position to open and close gripper

    Args:

        gripper:        group_name of gripper in the manipulator



        gripper_action: action to perform. Allowed values ["gripper_open", "gripper_close"]



    Returns:

        None.

    '''

    execute_trajectory_client = actionlib.SimpleActionClient('execute_trajectory', ExecuteTrajectoryAction)

    execute_trajectory_client.wait_for_server()

    

    gripper.set_named_target(gripper_action)

    plan_success, plan, planning_time, error_code = gripper.plan()

    goal=ExecuteTrajectoryGoal()

    goal.trajectory = plan

    execute_trajectory_client.send_goal(goal)

    execute_trajectory_client.wait_for_result()

    rospy.loginfo('\033[32m' + "Now at: {}".format(gripper_action) + '\033[0m')



def gripper_action(gripper_end, action="open"):

def gripper_action(gripper_end, action="open"):

    '''

    '''

    Function to execute the required action of the gripper.

    Function to execute the required action of the gripper.
@@ -112,13 +136,13 @@ def gripper_action(gripper_end, action="open"): 
    print("Executing gripper action = ", action)

    print("Executing gripper action = ", action)

    

    

    if(action == "open"):

    if(action == "open"):

        gripper_end.move(gripper_end.max_bound(), True)

        set_gripper_position(gripper_end,"gripper_open")

    elif(action == "close"):

    elif(action == "close"):

        gripper_end.move(gripper_end.max_bound()*0.5, True)

        set_gripper_position(gripper_end,"gripper_close")

    else:

    else:

        print("Action undefined") 

        print("Action undefined") 





def move_to_pose(arm, goal_pose):

def move_to_pose(arm, goal_pose, goal_angle):

    '''

    '''

    Function to move the robot arm to the requested pose

    Function to move the robot arm to the requested pose
@@ -131,18 +155,15 @@ def move_to_pose(arm, goal_pose): 
        None

        None

    '''

    '''

    #NOTE:

    #NOTE:

    #The planner will create a plan for the panda_link8 to reach

    #The planner will create a plan for the gripper_center to reach

    #the goal pose. But since we have an end-endeffector attached to it

    #the goal pose.

    #it will cause collision.



    #distance between wrist(panda_link8) and gripper_end(panda_hand_tcp)

    wrist_to_tcp = 0.103





    # rospy.logwarn("planning and moving to location")

    # rospy.logwarn("planning and moving to location")

    gripper_angle = geometry_msgs.msg.Quaternion()

    gripper_angle = geometry_msgs.msg.Quaternion()





    #Orientation of the tcp(panda_hand_tcp) w.r.t base frame(panda_link0) 

    #Orientation of the tcp(gripper_center) w.r.t base frame(world) 

    quaternion = tf.transformations.quaternion_from_euler(math.radians(180), 0, math.radians(-45))

    rpy = [math.radians(goal_angle[0]), math.radians(goal_angle[1]), math.radians(goal_angle[2])]

    quaternion = tf.transformations.quaternion_from_euler(rpy[0], rpy[1], rpy[2])





    gripper_angle.x = quaternion[0]

    gripper_angle.x = quaternion[0]

    gripper_angle.y = quaternion[1]

    gripper_angle.y = quaternion[1]
@@ -151,16 +172,16 @@ def move_to_pose(arm, goal_pose): 




    #defining pre-grasp position

    #defining pre-grasp position

    grasp_pose = Pose()

    grasp_pose = Pose()

    grasp_pose.position.x = goal_pose.position.x

    grasp_pose.position.x = goal_pose[0] #x position of the goal pose

    grasp_pose.position.y = goal_pose.position.y

    grasp_pose.position.y = goal_pose[1] #y position of the goal pose

    #adding tcp distance because the planning frame is panda_link8

    grasp_pose.position.z = goal_pose[2] 

    grasp_pose.position.z = goal_pose.position.z + wrist_to_tcp  

    grasp_pose.orientation = gripper_angle

    grasp_pose.orientation = gripper_angle

    

    

    print("Exectuting move_to_pose ({} , {},  {})"

    print("Exectuting move_to_pose ({} , {},  {}), ({}, {}, {})"

            .format(grasp_pose.position.x, grasp_pose.position.y, grasp_pose.position.z))

            .format(grasp_pose.position.x, grasp_pose.position.y, grasp_pose.position.z, 

                    goal_angle[0], goal_angle[1], goal_angle[2]))

    

    

    arm.set_goal_position_tolerance(0.001) #setting tolerence for pose 

    arm.set_goal_position_tolerance(0.01) #setting tolerence for pose 

    arm.set_pose_target(grasp_pose) #setting the pose of the end-effector

    arm.set_pose_target(grasp_pose) #setting the pose of the end-effector





    plan = arm.go(wait=True) #move the arm to the grasp_pose.

    plan = arm.go(wait=True) #move the arm to the grasp_pose.
@@ -188,11 +209,11 @@ def initialize_robot_params(): 
    rospy.loginfo("--- MoveIt APIs initialising ---")

    rospy.loginfo("--- MoveIt APIs initialising ---")

    scene = moveit_commander.PlanningSceneInterface() 

    scene = moveit_commander.PlanningSceneInterface() 

    robot_commander = moveit_commander.RobotCommander()

    robot_commander = moveit_commander.RobotCommander()

    gripper = robot_commander.get_joint('panda_finger_joint1')

    gripper = moveit_commander.MoveGroupCommander("gripper_group")





    rospy.sleep(3) #essential to intialize the API's

    rospy.sleep(3) #essential to intialize the API's





    group_name = "panda_arm" #group name of the robot arm we are using

    group_name = "arm_group" #group name of the robot arm we are using

   

   

    move_group = moveit_commander.MoveGroupCommander(group_name)

    move_group = moveit_commander.MoveGroupCommander(group_name)

    move_group.set_goal_position_tolerance(1E-2)

    move_group.set_goal_position_tolerance(1E-2)
@@ -247,16 +268,17 @@ def main(): 
    rospy.loginfo("------------------")

    rospy.loginfo("------------------")

    

    

    rospy.loginfo("Moving to pose1")

    rospy.loginfo("Moving to pose1")

    pose1 = Pose()

    

    pose1.position.x = 0.5 #x positin in base frame

    position1 = [0.231, -0.022, 0.103]

    pose1.position.y = 0.0

    orientation1 = [-90.174, 41.199, -5.554]

    pose1.position.z = 0.5

    move_to_pose(arm, position1, orientation1) #sending goal pose

    move_to_pose(arm, pose1) #sending goal pose





    rospy.loginfo("Moving to pose2")

    rospy.loginfo("Moving to pose2")

    pose2 = copy.deepcopy(pose1) #making a copy of the pose

    position2 = position1 #copying the position from pose1

    pose2.position.x += -1 #creating a different x value

    orientation2 = [-89.870, 41.196, 173.669] #copying the orientation from pose1

    move_to_pose(arm, pose2) #sending the goal pose

    position2[0] = -1*position2[0] #creating a different x value

    position2[1] = -1*position2[1] #creating a different y value

    move_to_pose(arm, position2, orientation2) #sending the goal pose

    

    

if __name__ == '__main__':

if __name__ == '__main__':

    #starting point of the code

    #starting point of the code

Exercises/python-exercises/3-Forward-Kinematics-DH-Table.ipynb

+1 −1
Original line number Original line Diff line number Diff line 
%% Cell type:markdown id:e4ccc978 tags:

%% Cell type:markdown id:e4ccc978 tags:





# Forward Kinematics - Denavit-Hartenberg Matrix

# Forward Kinematics - Denavit-Hartenberg Matrix





## Task Objective

## Task Objective

The aim of this task will be to use your understanding of the `sympy` package from the previous exercise and use the concept of **DH Table** to compute tranformation matrices that will give you the pose information of each link. You can get a **total of 20 points** if you complete the entire task successfully. You will receive partial points as per your implementation. The points for each task is mentioned in the comments in the format `(Points: *)`

The aim of this task will be to use your understanding of the `sympy` package from the previous exercise and use the concept of **DH Table** to compute tranformation matrices that will give you the pose information of each link. You can get a **total of 20 points** if you complete the entire task successfully. You will receive partial points as per your implementation. The points for each task is mentioned in the comments in the format `(Points: *)`





%% Cell type:markdown id:79d129c2 tags:

%% Cell type:markdown id:79d129c2 tags:





### Exercise 1:

### Exercise 1:

![task1](task_questions/task1.png)

![task description](task_questions/task-3.png)





%% Cell type:markdown id:d138648a tags:

%% Cell type:markdown id:d138648a tags:













The frame attachment information and the DH table of the Puma 560 robot are given below.

The frame attachment information and the DH table of the Puma 560 robot are given below.





The **constant parameters** are

The **constant parameters** are

- $h$ = 40 cm,

- $h$ = 40 cm,

- $a_2$ = 50 cm,

- $a_2$ = 50 cm,

- $a_3$ = 10 cm,

- $a_3$ = 10 cm,

- $d_3$ = 20 cm and

- $d_3$ = 20 cm and

- $d_4$ = 30 cm.

- $d_4$ = 30 cm.





1. Find transformation matrices between every consectuive frame.

1. Find transformation matrices between every consectuive frame.





   **Remember**:

   **Remember**:





   ${}_i^{i-1}T =

   ${}_i^{i-1}T =

   \begin{bmatrix}

   \begin{bmatrix}

    c_{\theta_i} & -s_{\theta_i} & 0 & a_i \\

    c_{\theta_i} & -s_{\theta_i} & 0 & a_i \\

    s_{\theta_i}c_{\alpha_i} & c_{\theta_i}c_{\alpha_i} & -s_{\alpha_i} & -s_{\alpha_i} d_i\\

    s_{\theta_i}c_{\alpha_i} & c_{\theta_i}c_{\alpha_i} & -s_{\alpha_i} & -s_{\alpha_i} d_i\\

    s_{\theta_i}s_{\alpha_i} & c_{\theta_i}s_{\alpha_i} & c_{\alpha_i} & c_{\alpha_i} d_i\\

    s_{\theta_i}s_{\alpha_i} & c_{\theta_i}s_{\alpha_i} & c_{\alpha_i} & c_{\alpha_i} d_i\\

    0 & 0 & 0 & 1\\

    0 & 0 & 0 & 1\\

    \end{bmatrix}$

    \end{bmatrix}$





   where $c_*, s_*$ represents `cos` and `sin` respectively.

   where $c_*, s_*$ represents `cos` and `sin` respectively.





%% Cell type:code id:856e8c26 tags:

%% Cell type:code id:856e8c26 tags:





``` python

``` python

# importing necessary modules

# importing necessary modules

import math

import math

import sympy as sym

import sympy as sym

#setting the precision for decimal values.

#setting the precision for decimal values.

#setting the precision for decimal values.

#setting the precision for decimal values.

%precision 4

%precision 4

sym.init_printing() #this will make all the symbolic print pretty.

sym.init_printing() #this will make all the symbolic print pretty.

```

```





%% Cell type:markdown id:3e160bf2 tags:

%% Cell type:markdown id:3e160bf2 tags:





Define your constant parameter **$h$**, **$a_i$** and **$d_i$**. You can define the variable parameters **$\theta_i$** using the [`Symbol`](https://www.tutorialspoint.com/sympy/sympy_symbols.htm) function available in the `sympy` library. **A quick start tutorial to the sympy library can be found [here](https://docs.sympy.org/latest/tutorials/intro-tutorial/gotchas.html).** We use the library [sympy](https://www.sympy.org/en/index.html) in the context of this task to allow for matrix multiplication with elements of the matrix being symbolic, since **the $\theta_i$ values are variable parameters.**

Define your constant parameter **$h$**, **$a_i$** and **$d_i$**. You can define the variable parameters **$\theta_i$** using the [`Symbol`](https://www.tutorialspoint.com/sympy/sympy_symbols.htm) function available in the `sympy` library. **A quick start tutorial to the sympy library can be found [here](https://docs.sympy.org/latest/tutorials/intro-tutorial/gotchas.html).** We use the library [sympy](https://www.sympy.org/en/index.html) in the context of this task to allow for matrix multiplication with elements of the matrix being symbolic, since **the $\theta_i$ values are variable parameters.**





%% Cell type:code id:2a7a4011 tags:

%% Cell type:code id:2a7a4011 tags:





``` python

``` python

#YOUR CODE HERE

#YOUR CODE HERE

#Define the constant parameters (Points: 2)

#Define the constant parameters (Points: 2)





#Define symbols for all theta

#Define symbols for all theta

#Example for theta_1 is shown below. You need to define other theta variables (Points: 2)

#Example for theta_1 is shown below. You need to define other theta variables (Points: 2)

theta_1 = sym.Symbol('theta_1')

theta_1 = sym.Symbol('theta_1')

```

```





%% Cell type:markdown id:023cc991 tags:

%% Cell type:markdown id:023cc991 tags:





Create a table to store the value of the DH table.

Create a table to store the value of the DH table.





**Hint:** Try using the [array](https://docs.sympy.org/latest/modules/tensor/array.html#module-sympy.tensor.array) function from the `sympy` library to create an N-dimensional array. Each row of the array will represent row from the DH table.

**Hint:** Try using the [array](https://docs.sympy.org/latest/modules/tensor/array.html#module-sympy.tensor.array) function from the `sympy` library to create an N-dimensional array. Each row of the array will represent row from the DH table.





%% Cell type:code id:6fb13c2f tags:

%% Cell type:code id:6fb13c2f tags:





``` python

``` python

#YOUR CODE HERE

#YOUR CODE HERE

#Fill in the array variable to match the table given in the question. (Points: 2)

#Fill in the array variable to match the table given in the question. (Points: 2)

#each row of the dh_table corresponds to each row from the DH table shown in the question

#each row of the dh_table corresponds to each row from the DH table shown in the question

dh_table = sym.Array([])

dh_table = sym.Array([])

```

```





%% Cell type:code id:13a74e36-b3e5-49d2-a741-a47e633fde6f tags:

%% Cell type:code id:13a74e36-b3e5-49d2-a741-a47e633fde6f tags:





``` python

``` python

#EVALUATION CELL

#EVALUATION CELL

#This cell is used to evaluate your implementation using some fixed values.

#This cell is used to evaluate your implementation using some fixed values.

#If this cell prints nothing then it means your implementation is correct.

#If this cell prints nothing then it means your implementation is correct.

#Otherwise this cell will print some error.

#Otherwise this cell will print some error.

assert dh_table.shape == (6, 4), "Shape is incorrect"

assert dh_table.shape == (6, 4), "Shape is incorrect"

```

```





%% Cell type:markdown id:af6b38c9 tags:

%% Cell type:markdown id:af6b38c9 tags:





Create a function `calc_transformation` that takes as input the values from each row of the DH table and then calculates the transformation matrix ${}_i^{i-1}T$ for each row of the DH Table.

Create a function `calc_transformation` that takes as input the values from each row of the DH table and then calculates the transformation matrix ${}_i^{i-1}T$ for each row of the DH Table.





**Hint:** Use the [Matrix](https://docs.sympy.org/latest/tutorials/intro-tutorial/matrices.html) and [trigonometric](https://docs.sympy.org/latest/modules/functions/elementary.html#trigonometric-functions) functions provided in the `sympy` package

**Hint:** Use the [Matrix](https://docs.sympy.org/latest/tutorials/intro-tutorial/matrices.html) and [trigonometric](https://docs.sympy.org/latest/modules/functions/elementary.html#trigonometric-functions) functions provided in the `sympy` package





%% Cell type:code id:5ae5f46a tags:

%% Cell type:code id:5ae5f46a tags:





``` python

``` python

'''

'''

Function to calculate the transformation matrix between frames `i` and `i-1`

Function to calculate the transformation matrix between frames `i` and `i-1`

Inputs:

Inputs:

    alpha, a, d, theta: values from the DH table

    alpha, a, d, theta: values from the DH table

Return:

Return:

    T: the transformation matrix between `i` and `i-1`

    T: the transformation matrix between `i` and `i-1`

'''

'''

def calc_transformation(alpha, a, d, theta):

def calc_transformation(alpha, a, d, theta):

    #Create the matrix (Points: 2)

    #Create the matrix (Points: 2)

    T = sym.Matrix()

    T = sym.Matrix()

    return T

    return T

```

```





%% Cell type:code id:fd3975c1 tags:

%% Cell type:code id:fd3975c1 tags:





``` python

``` python

n_frame = dh_table.shape[0] #shape() function gives a tuple with values row, columns

n_frame = dh_table.shape[0] #shape() function gives a tuple with values row, columns

T_list = []  #array to store the transformation matrices

T_list = []  #array to store the transformation matrices





for i in range(0,n_frame):

for i in range(0,n_frame):

    T = calc_transformation(alpha=dh_table[i,0], a=dh_table[i,1], d=dh_table[i,2], theta=dh_table[i,3])

    T = calc_transformation(alpha=dh_table[i,0], a=dh_table[i,1], d=dh_table[i,2], theta=dh_table[i,3])

    T_list.append(T)

    T_list.append(T)

```

```





%% Cell type:markdown id:56230bbd tags:

%% Cell type:markdown id:56230bbd tags:





Print the transformation matrix ${}_1^0T$

Print the transformation matrix ${}_1^0T$





**Note:** Do not use the python's `print` function while printing the matrix. This will make sure you have a pretty rendering of the matrix. You can try using the `print` function to see the difference.

**Note:** Do not use the python's `print` function while printing the matrix. This will make sure you have a pretty rendering of the matrix. You can try using the `print` function to see the difference.





If your implementation was correct, then **your result for the transformation ${}_1^0T$ will look like following matrix**.

If your implementation was correct, then **your result for the transformation ${}_1^0T$ will look like following matrix**.

${}_1^0T =

${}_1^0T =

\begin{bmatrix}

\begin{bmatrix}

cos(\theta_1) & -sin(\theta_1) & 0 & 0 \\

cos(\theta_1) & -sin(\theta_1) & 0 & 0 \\

sin(\theta_1) & cos(\theta_1) & 0 & 0\\

sin(\theta_1) & cos(\theta_1) & 0 & 0\\

0 & 0 & 1 & 0.4\\

0 & 0 & 1 & 0.4\\

0 & 0 & 0 & 1\\

0 & 0 & 0 & 1\\

\end{bmatrix}$

\end{bmatrix}$





%% Cell type:code id:fb9fff68 tags:

%% Cell type:code id:fb9fff68 tags:





``` python

``` python

#Print statement here

#Print statement here

#Note that we are not using the `print` function to print the value

#Note that we are not using the `print` function to print the value

#Print transformation matrix (Points: 2)

#Print transformation matrix (Points: 2)

T_list[0]

T_list[0]

```

```





%% Cell type:markdown id:2fbdb3d4 tags:

%% Cell type:markdown id:2fbdb3d4 tags:





Print the transformation matrix ${}_2^1T$:

Print the transformation matrix ${}_2^1T$:





%% Cell type:code id:aee3978b tags:

%% Cell type:code id:aee3978b tags:





``` python

``` python

#Print statement here

#Print statement here

```

```





%% Cell type:markdown id:08d7ff99 tags:

%% Cell type:markdown id:08d7ff99 tags:





Print the transformation matrix ${}_3^2T$:

Print the transformation matrix ${}_3^2T$:





%% Cell type:code id:4e2ceb82 tags:

%% Cell type:code id:4e2ceb82 tags:





``` python

``` python

#Print statement here

#Print statement here

```

```





%% Cell type:markdown id:66b1f49b tags:

%% Cell type:markdown id:66b1f49b tags:





Print the transformation matrix ${}_4^3T$:

Print the transformation matrix ${}_4^3T$:





%% Cell type:code id:15a390dc tags:

%% Cell type:code id:15a390dc tags:





``` python

``` python

#Print statement here

#Print statement here

```

```





%% Cell type:markdown id:5f1a09eb tags:

%% Cell type:markdown id:5f1a09eb tags:





Print the transformation matrix ${}_5^4T$:

Print the transformation matrix ${}_5^4T$:





%% Cell type:code id:a2bda339 tags:

%% Cell type:code id:a2bda339 tags:





``` python

``` python

#Print statement here

#Print statement here

```

```





%% Cell type:markdown id:139344a9 tags:

%% Cell type:markdown id:139344a9 tags:





Print the transformation matrix ${}_6^5T$:

Print the transformation matrix ${}_6^5T$:





%% Cell type:code id:71646159 tags:

%% Cell type:code id:71646159 tags:





``` python

``` python

#Print statement here

#Print statement here

```

```





%% Cell type:markdown id:9ec76206-53f3-49c9-b6a5-dd0b5019f5db tags:

%% Cell type:markdown id:9ec76206-53f3-49c9-b6a5-dd0b5019f5db tags:





## Calculate the angular jacobian matrix.

## Calculate the angular jacobian matrix.





2. Find the angular Jacobian matrix ($J_{\omega}$) between the world frame and the end-effector.

2. Find the angular Jacobian matrix ($J_{\omega}$) between the world frame and the end-effector.





   **Remember**:

   **Remember**:





   $J_{\omega} = \begin{bmatrix}

   $J_{\omega} = \begin{bmatrix}

   {}_1^0R \begin{bmatrix}0\\0\\1\end{bmatrix} & {}_2^0R \begin{bmatrix}0\\0\\1\end{bmatrix} & {}_3^0R \begin{bmatrix}0\\0\\1\end{bmatrix} & {}_4^0R \begin{bmatrix}0\\0\\1\end{bmatrix}  & {}_5^0R \begin{bmatrix}0\\0\\1\end{bmatrix} & {}_6^0R \begin{bmatrix}0\\0\\1\end{bmatrix}

   {}_1^0R \begin{bmatrix}0\\0\\1\end{bmatrix} & {}_2^0R \begin{bmatrix}0\\0\\1\end{bmatrix} & {}_3^0R \begin{bmatrix}0\\0\\1\end{bmatrix} & {}_4^0R \begin{bmatrix}0\\0\\1\end{bmatrix}  & {}_5^0R \begin{bmatrix}0\\0\\1\end{bmatrix} & {}_6^0R \begin{bmatrix}0\\0\\1\end{bmatrix}

   \end{bmatrix}$

   \end{bmatrix}$





Steps to follow:

Steps to follow:





1. Get the rotation matrix from the transformation matrix.

1. Get the rotation matrix from the transformation matrix.

2. Do a matrix-vector multiplication between the **rotation matrix** and the vector

2. Do a matrix-vector multiplication between the **rotation matrix** and the vector

$\begin{bmatrix} 0 \\ 0\\ 1 \end{bmatrix}$ . This matrix-vector multiplication will produce a `3x1` vector which we will concatenate together to form the final jacobian $J_{\omega}$

$\begin{bmatrix} 0 \\ 0\\ 1 \end{bmatrix}$ . This matrix-vector multiplication will produce a `3x1` vector which we will concatenate together to form the final jacobian $J_{\omega}$





**Some useful tips:**

**Some useful tips:**





If you define a single row in the `Matrix` function of the `sympy` package, then it will be taken as a vector. **Do not get confused between using the same `Matrix` function to create both a matrix and a vector**. The number of rows you define will decide if you are creating a matrix of a vector.

If you define a single row in the `Matrix` function of the `sympy` package, then it will be taken as a vector. **Do not get confused between using the same `Matrix` function to create both a matrix and a vector**. The number of rows you define will decide if you are creating a matrix of a vector.

- 1 row -> Vector

- 1 row -> Vector

- More than 1 row -> Matrix

- More than 1 row -> Matrix





You can use the `@` operator available in the `sympy` package to do `Matrix-vector` multiplication.

You can use the `@` operator available in the `sympy` package to do `Matrix-vector` multiplication.





%% Cell type:code id:242bc822-ad2c-4cfe-9f6a-d67489a233dc tags:

%% Cell type:code id:242bc822-ad2c-4cfe-9f6a-d67489a233dc tags:





``` python

``` python

#example of how to define a vector.

#example of how to define a vector.

vec_1 = sym.Matrix([1,2,3])

vec_1 = sym.Matrix([1,2,3])





#printing the vector. Note that you will get a pretty looking vector form if you don't use the print() function.

#printing the vector. Note that you will get a pretty looking vector form if you don't use the print() function.

#uncomment the following line to print the vector

#uncomment the following line to print the vector

#vec_1

#vec_1





#matric vector multiplication.

#matric vector multiplication.

mat_ = sym.Matrix([[1,2,3],

mat_ = sym.Matrix([[1,2,3],

                   [4,5,6],

                   [4,5,6],

                   [7,8,9]]) #pay attention to more rows we have added here

                   [7,8,9]]) #pay attention to more rows we have added here





result = mat_@vec_1 # <--- matrix vector multiplication

result = mat_@vec_1 # <--- matrix vector multiplication

result #printing the result of matrix-vec multiplication

result #printing the result of matrix-vec multiplication

```

```





%% Cell type:code id:a10e031d-cfeb-4839-b6e0-2a0b7d906675 tags:

%% Cell type:code id:a10e031d-cfeb-4839-b6e0-2a0b7d906675 tags:





``` python

``` python

#You can compute the cross product of two vectors using the `cross` function as shown below.

#You can compute the cross product of two vectors using the `cross` function as shown below.

vec_2 = sym.Matrix([4,5,6])

vec_2 = sym.Matrix([4,5,6])

cross_prod = vec_1.cross(vec_2)

cross_prod = vec_1.cross(vec_2)

cross_prod #printing the cross product

cross_prod #printing the cross product

```

```





%% Cell type:markdown id:24239c3e-f2a7-40f7-92b7-421b5c54e0e4 tags:

%% Cell type:markdown id:24239c3e-f2a7-40f7-92b7-421b5c54e0e4 tags:









**Step 1:**

**Step 1:**





Create a function `get_rotation_matrix` that takes as input the transformation matrix and return the `3x3` rotation matrix from the transformation matrix.

Create a function `get_rotation_matrix` that takes as input the transformation matrix and return the `3x3` rotation matrix from the transformation matrix.





***Hint:*** You can use [matrix slicing](https://docs.sympy.org/latest/modules/matrices/matrices.html#basic-manipulation:~:text=also%20able%20to-,slice%20submatrices,-%3B%20slices%20always%20give) to easily obtain the rotation matrix.

***Hint:*** You can use [matrix slicing](https://docs.sympy.org/latest/modules/matrices/matrices.html#basic-manipulation:~:text=also%20able%20to-,slice%20submatrices,-%3B%20slices%20always%20give) to easily obtain the rotation matrix.





%% Cell type:code id:b0d16468-94e2-4a11-b7ed-6092a004a679 tags:

%% Cell type:code id:b0d16468-94e2-4a11-b7ed-6092a004a679 tags:





``` python

``` python

def get_rotation_matrix(tf_matrix):

def get_rotation_matrix(tf_matrix):

    #YOUR CODE HERE (points: 2)

    #YOUR CODE HERE (points: 2)

    #rot_matrix=

    #rot_matrix=

    return rot_matrix

    return rot_matrix

```

```





%% Cell type:markdown id:ee1a342f-940c-403b-9b92-e5719b54c060 tags:

%% Cell type:markdown id:ee1a342f-940c-403b-9b92-e5719b54c060 tags:





**Step 2:**

**Step 2:**





- Create a loop that will recurcively get the rotation matrix from the transformation matrix.

- Create a loop that will recurcively get the rotation matrix from the transformation matrix.

- Then compute the matrix-vector multiplication.

- Then compute the matrix-vector multiplication.

- Finally push the vector to the corresponding column of the jacobian matrix.

- Finally push the vector to the corresponding column of the jacobian matrix.





%% Cell type:code id:bcd202f7-9fd5-4834-937e-81b28d6e019b tags:

%% Cell type:code id:bcd202f7-9fd5-4834-937e-81b28d6e019b tags:





``` python

``` python

angular_jacobian = sym.zeros(3,6) #creating a zero matrix to update the final value

angular_jacobian = sym.zeros(3,6) #creating a zero matrix to update the final value





for i in range(0, len(T_list)):

for i in range(0, len(T_list)):

    #1.compute the rotation matrix using the `get_rotation_matrix` function. (point: 2)

    #1.compute the rotation matrix using the `get_rotation_matrix` function. (point: 2)

    #rotation_matrix =

    #rotation_matrix =





    #2.create the vector [0,0,1] (point: 2)

    #2.create the vector [0,0,1] (point: 2)

    #unit_vec =

    #unit_vec =





    #3.compute the matrix-vector multiplication (point: 2)

    #3.compute the matrix-vector multiplication (point: 2)

    #result_ =

    #result_ =





    #4.push the resulting vector to the corresponding colum of the jacobian matrix. (point: 2)

    #4.push the resulting vector to the corresponding colum of the jacobian matrix. (point: 2)

    angular_jacobian[:,i] = result_

    angular_jacobian[:,i] = result_





#printing the final angular_jacobian

#printing the final angular_jacobian

angular_jacobian

angular_jacobian

```
 
```

Exercises/python-exercises/task_questions/task-3.png

0 → 100644
+128 KiB

127.54 KiB

Exercises/python-exercises/task_questions/task1.png

deleted100644 → 0
−496 KiB

496.46 KiB