Skip to content
Snippets Groups Projects
Commit 81d40199 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Add the possibility to have half-sitting, flexibility and fixed

robot in the world.
parent bf8d7a38
No related branches found
No related tags found
No related merge requests found
Pipeline #7765 failed
......@@ -15,6 +15,18 @@ Some dummy data were added (rotor inertia) to help the dynamic regularization.
For initial and data validated by PAL-Robotics please see the http://github.com/pal-robotics/talos_robot
repository.
## Fixed joint
To start the robot in the air you can use:
```
roslaunch talos_data talos_gazebo.launch enable_fixed_robot:=true
```
## Straight robot
To start the robot straight (i.e. not in half-sitting) you can use:
```
roslaunch talos_data talos_gazebo.launch starting_half_sitting:=false
```
## Flexibilities
To start the flexibilities:
......
zeros:
torso_1_joint: 0.0
torso_2_joint: 0.006761
head_1_joint: 0.0
head_2_joint: 0.0
arm_left_1_joint: 0.25847
arm_left_2_joint: 0.173046
arm_left_3_joint: 0.0002
arm_left_4_joint: -0.525366
arm_left_5_joint: 0.0
arm_left_6_joint: 0.0
arm_left_7_joint: 0.1
arm_right_1_joint: -0.25847
arm_right_2_joint: -0.173046
arm_right_3_joint: -0.0002
arm_right_4_joint: -0.525366
arm_right_5_joint: 0.0
arm_right_6_joint: 0.0
arm_right_7_joint: 0.1
gripper_left_joint: 0.0
gripper_left_inner_double_joint: 0.0
gripper_left_fingertip_1_joint: 0.0
gripper_left_fingertip_2_joint: 0.0
gripper_left_motor_single_joint: 0.0
gripper_left_inner_single_joint: 0.0
gripper_left_fingertip_3_joint: 0.0
gripper_right_joint: 0.0
gripper_right_inner_double_joint: 0.0
gripper_right_fingertip_1_joint: 0.0
gripper_right_fingertip_2_joint: 0.0
gripper_right_motor_single_joint: 0.0
gripper_right_inner_single_joint: 0.0
gripper_right_fingertip_3_joint: 0.0
leg_left_1_joint: 0.0
leg_left_2_joint: 0.0
leg_left_3_joint: -0.411354
leg_left_4_joint: 0.859395
leg_left_5_joint: -0.448041
leg_left_6_joint: -0.001708
leg_right_1_joint: 0.0
leg_right_2_joint: 0.0
leg_right_3_joint: -0.411354
leg_right_4_joint: 0.859395
leg_right_5_joint: -0.448041
leg_right_6_joint: -0.001708
default_floating_base_pose:
orientation:
x: 0.
y: 0.
z: 0.
w: 1.
position: [0., 0., 1.0]
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="start_half_sitting" default="true"/>
<arg name="world" default="empty"/> <!-- empty, small_office, ... (see ../worlds) -->
<arg name="gzpose" default="-x 0.0 -y 0.0 -z 1.16 -R 0.0 -P 0.0 -Y 0.0"/>
<arg name="gzpose" default="-x 0.0 -y 0.0 -z 1.16 -R 0.0 -P 0.0 -Y 0.0" unless="$(arg start_half_sitting)"/>
<arg name="gui" default="true"/>
<arg name="debug" default="false"/>
<arg name="recording" default="false"/>
......@@ -11,11 +12,19 @@
<arg name="load_model" default="true"/> <!-- AS: should probably be false by default -->
<arg name="robot" default="full_v2"/> <!-- full, lower_body, foot -->
<arg name="foot_collision" default="thinbox"/>
<arg name="enable_leg_passive" default="true"/>
<arg name="enable_leg_passive" default="false"/>
<arg name="enable_fixed_robot" default="false"/>
<arg name="default_configuration_type" default="zeros"/>
<env name="GAZEBO_MODEL_PATH" value="$(find talos_gazebo)/models:$(optenv GAZEBO_MODEL_PATH)"/>
<arg name="left_leg_pose" default="-J leg_left_1_joint 0.0 -J leg_left_2_joint 0.0 -J leg_left_3_joint -0.411354 -J leg_left_4_joint 0.859395 -J leg_left_5_joint -0.448041 -J leg_left_6_joint -0.001708"/>
<arg name="right_leg_pose" default="-J leg_right_1_joint 0.0 -J leg_right_2_joint 0.0 -J leg_right_3_joint -0.411354 -J leg_right_4_joint 0.859395 -J leg_right_5_joint -0.448041 -J leg_right_6_joint -0.001708"/>
<arg name="left_arm_pose" default="-J arm_left_1_joint 0.25847 -J arm_left_2_joint 0.173046 -J arm_left_3_joint -0.0002 -J arm_left_4_joint -0.525366 -J arm_left_5_joint 0.0 -J arm_left_6_joint 0.0 -J arm_left_7_joint 0.1"/>
<arg name="right_arm_pose" default="-J arm_right_1_joint -0.25847 -J arm_right_2_joint -0.173046 -J arm_left_3_joint 0.0002 -J arm_right_4_joint -0.525366 -J arm_right_5_joint 0.0 -J arm_right_6_joint 0.0 -J arm_right_7_joint 0.1"/>
<arg name="torso_pose" default="-J torso_1_joint 0.0 -J torso_2_joint 0.006761"/>
<arg name="gzpose" default="-x 0.0 -y 0.0 -z 1.00 -R 0.0 -P 0.0 -Y 0.0 $(arg left_leg_pose) $(arg right_leg_pose) $(arg left_arm_pose) $(arg right_arm_pose) $(arg torso_pose) -u" if="$(arg start_half_sitting)"/>
<!-- start up world -->
<include file="$(find pal_gazebo_worlds)/launch/pal_gazebo.launch">
<arg name="world" value="$(arg world)"/>
......@@ -31,6 +40,7 @@
<arg name="robot" value="$(arg robot)"/>
<arg name="foot_collision" default="$(arg foot_collision)"/>
<arg name="enable_leg_passive" default="$(arg enable_leg_passive)"/>
<arg name="enable_fixed_robot" default="$(arg enable_fixed_robot)"/>
<arg name="default_configuration_type" default="$(arg default_configuration_type)"/>
</include>
</group>
......
......@@ -10,6 +10,7 @@
<!-- TODO AS: zeros should not be the default (self-colliding), but it is currently required for tests -->
<arg name="default_configuration_type" default="zeros"/>
<arg name="enable_leg_passive" default="true"/>
<arg name="enable_fixed_robot" default="true"/>
<!-- Robot description -->
<param
......@@ -18,7 +19,8 @@
'$(find talos_data)/robots/talos_$(arg robot).urdf.xacro'
foot_collision:=$(arg foot_collision)
enable_crane:=$(arg enable_crane)
enable_leg_passive:=$(arg enable_leg_passive)"
enable_leg_passive:=$(arg enable_leg_passive)
enable_fixed_robot:=$(arg enable_fixed_robot)"
/>
<rosparam command="load" file="$(find talos_data)/config/default_configuration_$(arg default_configuration_type).yaml" />
......
......@@ -105,14 +105,14 @@
</collision>
</link>
<!-- <xacro:if value="$(arg enable_fixed_robot)"> -->
<xacro:if value="$(arg enable_fixed_robot)">
<link name="world"/>
<joint name="fixed" type="fixed">
<parent link="world"/>
<child link="base_link"/>
</joint>
<!-- </xacro:if> -->
</xacro:if>
<joint name="${name}_2_joint" type="revolute">
<parent link="${name}_1_link"/>
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment