Skip to content

Commit

Permalink
Merge pull request #3 from IntelligentRoboticsLabs/humble-devel
Browse files Browse the repository at this point in the history 
New world with focuses and parameters to modify the world
  • Loading branch information
@Juancams
Juancams authored Jun 13, 2024
2 parents 6bab562 + 523e993 commit 6bb7265
Show file tree
Hide file tree
Showing 11 changed files with 1,014 additions and 134 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@ endif()

find_package(ament_cmake REQUIRED)

install(DIRECTORY config launch worlds rviz urdf
install(DIRECTORY config launch worlds rviz urdf params
DESTINATION share/${PROJECT_NAME}
)

Expand Down
178 changes: 153 additions & 25 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@ Simulator for guidance, navigation and control of proximity orbital operations.
- [Instalation](#instalation)
- [Launch the simulation environment](#launch-the-simulation-environment)
- [Interfaces](#interfaces)
- [Configuration](#configuration)
- [Usage](#usage)
- [Demos](#demos)
- [About](#about)
Expand Down Expand Up @@ -57,7 +58,7 @@ ros2 launch gnc_orbital ur_sim_control.launch.py

Once launched, the rviz viewer with both arms, camera and rail, and the gazebo simulation environment should open.

![Screenshot from 2024-05-27 11-48-34](https://github.com/IntelligentRoboticsLabs/GNC_orbital/assets/44479765/83673c64-a232-448d-9c75-023ef940c916)
![simulation](https://github.com/IntelligentRoboticsLabs/GNC_orbital/assets/44479765/451b97c9-ce72-4c17-8f82-575c2d83de9f)

If what you want is for the rviz or gazebo viewer not to open, use one of these two commands or a combination of both:

Expand Down Expand Up @@ -87,31 +88,30 @@ Once the simulation is launched, you can use the `ros2 topic list` command to ob
/rosout
/tf
/tf_static
/ur10_camera/camera_info
/ur10_camera/depth/camera_info
/ur10_camera/depth/image_raw
/ur10_camera/depth/image_raw/compressed
/ur10_camera/depth/image_raw/compressedDepth
/ur10_camera/depth/image_raw/theora
/ur10_camera/image_raw
/ur10_camera/image_raw/compressed
/ur10_camera/image_raw/compressedDepth
/ur10_camera/image_raw/theora
/ur10_camera/points
/ur10_joint_state_broadcaster/transition_event
/ur10_joint_trajectory_controller/controller_state
/ur10_joint_trajectory_controller/joint_trajectory
/ur10_joint_trajectory_controller/state
/ur10_joint_trajectory_controller/transition_event
/ur5_camera/camera_info
/ur5_camera/depth/camera_info
/ur5_camera/depth/image_raw
/ur5_camera/depth/image_raw/compressed
/ur5_camera/depth/image_raw/compressedDepth
/ur5_camera/depth/image_raw/theora
/ur5_camera/image_raw
/ur5_camera/image_raw/compressed
/ur5_camera/image_raw/compressedDepth
/ur5_camera/image_raw/theora
/ur5_camera/imu
/ur5_camera/points
/ur5_joint_state_broadcaster/transition_event
/ur5_joint_trajectory_controller/controller_state
/ur5_joint_trajectory_controller/joint_trajectory
/ur5_joint_trajectory_controller/state
/ur5_joint_trajectory_controller/transition_event
/ur3_joint_state_broadcaster/transition_event
/ur3_joint_trajectory_controller/controller_state
/ur3_joint_trajectory_controller/joint_trajectory
/ur3_joint_trajectory_controller/state
/ur3_joint_trajectory_controller/transition_event
```

You have the topics for each of the robots, where you can see the status of each of the joints of each arm, or the image from the camera that the ur5 has. Additionally, you will be able to see all the transforms in /tf or /tf_static
You have the topics for each of the robots, where you can see the status of each of the joints of each arm, or the image from the camera that the ur10 has. Additionally, you will be able to see all the transforms in /tf or /tf_static

</details>

Expand All @@ -122,23 +122,148 @@ Once the simulation is launched, you can use the `ros2 action list` command to o

```bash
/ur10_joint_trajectory_controller/follow_joint_trajectory
/ur5_joint_trajectory_controller/follow_joint_trajectory
/ur3_joint_trajectory_controller/follow_joint_trajectory
```

These are the actions that moveit2 will use to be able to move the arms

</details>

## Configuration
### Spotlights
In the file `params/world_params.yaml` you will find the world configurations, including the spotlights.

<details>
<summary>world_params.yaml</summary>

```yaml
# Spotlight 1
## Pose
spot1_x_light: 0.0
spot1_y_light: 0.0
spot1_z_light: 1.2
spot1_roll_light: 0.0
spot1_pitch_light: 1.56
spot1_yaw_light: 0.78

## Diffuse
spot1_R_light: 1
spot1_G_light: 1
spot1_B_light: 1
spot1_opacity_light: 1.0

## Specular
spot1_specular_R: 1
spot1_specular_G: 1
spot1_specular_B: 1
spot1_specular_opacity: 1.0

## Attenuation
spot1_attenuation_range: 30
spot1_attenuation_linear: 1
spot1_quadratic: 0.001

## Spot
spot1_inner_angle: 0.1
spot1_outer_angle: 0.2
spot1_fall_off: 0.5


# Spotlight 2
## Pose
spot2_x_light: -2.0
spot2_y_light: 0.0
spot2_z_light: 1.2
spot2_roll_light: 0.0
spot2_pitch_light: 1.56
spot2_yaw_light: 2.53

## Diffuse
spot2_R_light: 1
spot2_G_light: 1
spot2_B_light: 1
spot2_opacity_light: 1.0

## Specular
spot2_specular_R: 1
spot2_specular_G: 1
spot2_specular_B: 1
spot2_specular_opacity: 1.0

## Attenuation
spot2_attenuation_range: 30
spot2_attenuation_linear: 1
spot2_quadratic: 0.001

## Spot
spot2_inner_angle: 0.1
spot2_outer_angle: 0.2
spot2_fall_off: 0.5
```
</details>
Parameters to take into account:
- To change the color of the spotlights, you have to modify the diffuse and specular RGB values, which range from 0 to 1.
- To modify the light range, you have to modify the spotlight's attenuation_range value.
- To modify the angle by which the light extends, you must touch the outer_angle value. This value cannot be less than the inner, so if a smaller angle is necessary, both will have to be modified.
- You can also modify the position of the spotlights and the angle at which the light goes, by touching the xyz rpy parameters.
- The parameters attenuation_linear, quadratic and fall_of are used to configure how the light decreases linearly with distance.
### Arms
The configuration of the arms is located in params/arms_params.yaml, and this allows you to modify its position and initial orientation, as well as the name given to it, type of arm and the prefix that will be given to the tf of the arm to differentiate them from another.
<details>
<summary>arms_params.yaml</summary>
```yaml
gnc_orbital:
arm1:
name: ur3
type: ur3
tf_prefix: ur3_
position: [-2.4, -1.6, 0.73]
orientation: [0, 0, 0]
arm2:
name: ur10
type: ur10
tf_prefix: ur10_
position: [0, -2.89, 1]
orientation: [-1.56, 0, 0]
```
</details>
It must be taken into account that if the table arm is going to be moved, the position of this table must also be modified. You can also do this from the params/world_params.yaml file
<details>
<summary>world_params.yaml</summary>
```yaml
# UR 5 Table
## Pose
x_table: -2.6
y_table: -1.1
z_table: 0.35
roll_table: 0.0
pitch_table: 0.0
yaw_table: 1.56

## Size
length_table: 1.2
width_table: 0.6
height_table: 0.73
```
</details>
## Usage
In order to move the ur5 robot, you will have to use the following command from terminal:
In order to move the ur3 robot, you will have to use the following command from terminal:
```bash
ros2 topic pub /ur5_joint_trajectory_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "header:
ros2 topic pub /ur3_joint_trajectory_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "header:
stamp:
sec: 0
nanosec: 0
frame_id: ''
joint_names: ['ur5_shoulder_pan_joint', 'ur5_shoulder_lift_joint', 'ur5_elbow_joint', 'ur5_wrist_1_joint', 'ur5_wrist_2_joint', 'ur5_wrist_3_joint']
joint_names: ['ur3_shoulder_pan_joint', 'ur3_shoulder_lift_joint', 'ur3_elbow_joint', 'ur3_wrist_1_joint', 'ur3_wrist_2_joint', 'ur3_wrist_3_joint']
points: [{positions: [0.0, -1.57, 1.57, 0.0, 0.0, 0.0],
time_from_start: {sec: 2, nanosec: 0}}]"
```
Expand All @@ -152,8 +277,8 @@ ros2 topic pub /ur10_joint_trajectory_controller/joint_trajectory trajectory_msg
nanosec: 0
frame_id: ''
joint_names: ['ur10_shoulder_pan_joint', 'ur10_shoulder_lift_joint', 'ur10_elbow_joint', 'ur10_wrist_1_joint', 'ur10_wrist_2_joint', 'ur10_wrist_3_joint', 'ur10_rail_joint']
points: [{positions: [0.0, -1.57, 1.57, 0.0, 0.0, 0.0, 0.6],
time_from_start: {sec: 2, nanosec: 0}}]"
points: [{positions: [0.0, -1.57, -1.57, 0.7, 1.5, -1.2, -1.1],
time_from_start: {sec: 2, nanosec: 0}}]"
```
If what you want is to see the image or the pointcloud generated by the camera, or the transform system, you should go to Rviz and follow the following steps:
Expand All @@ -167,6 +292,9 @@ In this demo the commands given above are being executed, only modifying the pos

[Demo 01: ur5 arm movement with camera and ur10 arm publishing in topics](https://github.com/IntelligentRoboticsLabs/GNC_orbital/assets/44479765/b95b3e50-8d5f-4296-9769-043cdaa1f76b)

[Demo 02: ur3 and ur10 arm movement with camera. Totally dark world with spotlights](https://github.com/IntelligentRoboticsLabs/GNC_orbital/assets/44479765/e13574ec-540a-47cf-b334-20a12cc766e9)


## About
This is a project made by the [Intelligent Robotics Lab], a research group from the [Universidad Rey Juan Carlos].
Copyright &copy; 2024.
Expand Down
70 changes: 67 additions & 3 deletions launch/ur_sim_control.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,15 @@
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import (
get_package_share_directory
)
import os
import yaml

def load_yaml_file(file_path):
with open(file_path, 'r') as file:
return yaml.safe_load(file)

def launch_setup(context, *args, **kwargs):

Expand All @@ -63,8 +71,39 @@ def launch_setup(context, *args, **kwargs):
[FindPackageShare(runtime_config_package), 'config', 'ur_controllers.yaml']
)

path_template = os.path.join(get_package_share_directory('gnc_orbital'), 'worlds', 'lab.sdf.template')
path_world = os.path.join(get_package_share_directory('gnc_orbital'), 'worlds', 'lab.sdf')

yaml_file_path = os.path.join(get_package_share_directory('gnc_orbital'), 'params', 'world_params.yaml')
config = load_yaml_file(yaml_file_path)

nodes_to_start = []

with open(path_template, 'r') as file:
world_content = file.read()

for key, value in config.items():
placeholder = f'{{{{{key}}}}}'
world_content = world_content.replace(placeholder, str(value))

if (key == 'spot1_z_light'):
world_content = world_content.replace('{{spot1_cylinder_z}}', str((value-0.05)/2))
world_content = world_content.replace('{{spot1_cylinder_length}}', str(value-0.05))

if (key == 'spot2_z_light'):
world_content = world_content.replace('{{spot2_cylinder_z}}', str((value-0.05)/2))
world_content = world_content.replace('{{spot2_cylinder_length}}', str(value-0.05))

with open(path_world, 'w') as file:
file.write(world_content)

arms_params_file = os.path.join(get_package_share_directory('gnc_orbital'), 'params', 'arms_params.yaml')
with open(arms_params_file, "r") as stream:
arms_params = (yaml.safe_load(stream))

arm1 = arms_params['gnc_orbital']['arm1']
arm2 = arms_params['gnc_orbital']['arm2']

robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name='xacro')]),
Expand All @@ -83,12 +122,37 @@ def launch_setup(context, *args, **kwargs):
safety_k_position,
' ',
'name:=',
'ur',
'gnc_orbital',
' ',
'name1:=',
arm1['name'],
' ',
'name2:=',
arm2['name'],
' ',
'ur_type1:=',
arm1['type'],
' ',
'ur_type2:=',
arm2['type'],
' ',
'tf_prefix1:=',
arm1['tf_prefix'],
' ',
'tf_prefix2:=',
arm2['tf_prefix'],
' ',
'arm1_pose:="', f"{arm1['position'][0]} {arm1['position'][1]} {arm1['position'][2]}", '"',
' ',
'arm1_orientation:="', f"{arm1['orientation'][0]} {arm1['orientation'][1]} {arm1['orientation'][2]}", '"',
' ',
'arm2_pose:="', f"{arm2['position'][0]} {arm2['position'][1]} {arm2['position'][2]}", '"',
' ',
'arm2_orientation:="', f"{arm2['orientation'][0]} {arm2['orientation'][1]} {arm2['orientation'][2]}", '"',
' ',
'sim_gazebo:=true',
' ',
'simulation_controllers:=',
initial_joint_controllers,
'simulation_controllers:=', initial_joint_controllers,
]
)
robot_description = {'robot_description': robot_description_content}
Expand Down
13 changes: 13 additions & 0 deletions params/arms_params.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
gnc_orbital:
arm1:
name: ur3
type: ur3
tf_prefix: ur3_
position: [-2.4, -1.6, 0.73]
orientation: [0, 0, 0]
arm2:
name: ur10
type: ur10
tf_prefix: ur10_
position: [0, -2.89, 1]
orientation: [-1.56, 0, 0]
Loading

0 comments on commit 6bb7265

Please sign in to comment.