tiago fixes

launch/giskardpy_tiago_standalone.launch

@@ -1,7 +1,9 @@
 <launch>
 
+ <include file="$(find iai_tiago_description)/launch/upload.launch"/>
+
  <node pkg="giskardpy" type="giskard.py" name="giskard" output="screen">
- <param name="config" value="IAI_Tiago"/>
+ <param name="config" value="Tiago_Standalone"/>
  </node>
 
  <node pkg="giskardpy" type="interactive_marker.py" name="giskard_interactive_marker" output="screen">
@@ -15,6 +17,4 @@
 
  <node pkg="giskardpy" type="move_base_simple_goal_diff_drive.py" name="my_move_base" output="screen" />
 
- <node pkg="iai_tiago_tools" type="giskard_e_stop.py" name="giskard_e_stop" output="screen" />
-
 </launch>

scripts/move_base_simple_goal_diff_drive.py

@@ -2,12 +2,14 @@
 import rospy
 from geometry_msgs.msg import PoseStamped
 
+from giskardpy.my_types import PrefixName
 from giskardpy.python_interface import GiskardWrapper
 
 
 def call_back(goal: PoseStamped):
  rospy.loginfo('received simple move base goal')
- tip_link = 'base_footprint'
+ robot_name = giskard.robot_name
+ tip_link = str(PrefixName('base_footprint', robot_name))
  root_link = 'map'
  giskard.set_json_goal(constraint_type='DiffDriveBaseGoal',
  tip_link=tip_link, root_link=root_link,

src/giskardpy/configs/default_giskard.py

@@ -175,6 +175,7 @@ def get_default_group_name(self):
  def add_diff_drive_joint(self,
  parent_link_name: str,
  child_link_name: str,
+ robot_group_name: Optional[str] = None,
  name: Optional[str] = 'brumbrum',
  odometry_topic: Optional[str] = None,
  translation_velocity_limit: Optional[float] = 0.2,
@@ -186,9 +187,12 @@ def add_diff_drive_joint(self,
  odom_x_name: Optional[str] = 'odom_x',
  odom_y_name: Optional[str] = 'odom_y',
  odom_yaw_name: Optional[str] = 'odom_yaw'):
+ if robot_group_name is None:
+ robot_group_name = self.get_default_group_name()
  brumbrum_joint = DiffDrive(parent_link_name=parent_link_name,
- child_link_name=child_link_name,
+ child_link_name=PrefixName(child_link_name, robot_group_name),
  name=name,
+ group_name=robot_group_name,
  odom_x_name=odom_x_name,
  odom_y_name=odom_y_name,
  odom_yaw_name=odom_yaw_name,

src/giskardpy/configs/tiago.py

@@ -10,55 +10,42 @@
 class TiagoBase(Giskard):
  def __init__(self):
  super().__init__()
- self._general_config.default_link_color = ColorRGBA(1, 1, 1, 0.7)
- self.collision_avoidance_config.load_moveit_self_collision_matrix(
- 'package:https://tiago_dual_moveit_config/config/srdf/tiago.srdf')
- self.collision_avoidance_config.overwrite_external_collision_avoidance('brumbrum',
- number_of_repeller=2,
- soft_threshold=0.2,
- hard_threshold=0.1)
- self.collision_avoidance_config.ignored_collisions = ['wheel_left_link',
- 'wheel_right_link',
- 'caster_back_left_2_link',
- 'caster_back_right_2_link',
- 'caster_front_left_2_link',
- 'caster_front_right_2_link']
- self.collision_avoidance_config.fix_joints_for_self_collision_avoidance(['head_1_joint',
- 'head_2_joint',
- 'gripper_left_left_finger_joint',
- 'gripper_left_right_finger_joint',
- 'gripper_right_left_finger_joint',
- 'gripper_right_right_finger_joint'])
- self.collision_avoidance_config.fix_joints_for_external_collision_avoidance(['gripper_left_left_finger_joint',
- 'gripper_left_right_finger_joint',
- 'gripper_right_left_finger_joint',
- 'gripper_right_right_finger_joint'])
- self.collision_avoidance_config.overwrite_external_collision_avoidance('arm_right_7_joint',
- number_of_repeller=4,
- soft_threshold=0.05,
- hard_threshold=0.0,
- max_velocity=0.2)
- self.collision_avoidance_config.overwrite_external_collision_avoidance('arm_left_7_joint',
- number_of_repeller=4,
- soft_threshold=0.05,
- hard_threshold=0.0,
- max_velocity=0.2)
- self.collision_avoidance_config.set_default_self_collision_avoidance(hard_threshold=0.04,
- soft_threshold=0.08)
- self.collision_avoidance_config.set_default_external_collision_avoidance(hard_threshold=0.03,
- soft_threshold=0.08)
- # self.general_config.joint_limits['jerk'] = defaultdict(lambda: 60)
- # self.collision_avoidance_config.ignore_self_collisions_of_pair('arm_right_3_link', 'torso_lift_link')
- # self.collision_avoidance_config.ignore_self_collisions_of_pair('arm_left_3_link', 'torso_lift_link')
- # self.collision_avoidance_config.ignore_self_collisions_of_pair('arm_right_2_link', 'torso_lift_link')
- # self.collision_avoidance_config.ignore_self_collisions_of_pair('arm_left_2_link', 'torso_lift_link')
- #
- # self.collision_avoidance_config.ignore_self_collisions_of_pair('arm_right_3_link', 'torso_fixed_column_link')
- # self.collision_avoidance_config.ignore_self_collisions_of_pair('arm_left_3_link', 'torso_fixed_column_link')
- # self.collision_avoidance_config.ignore_self_collisions_of_pair('arm_right_2_link', 'torso_fixed_column_link')
- # self.collision_avoidance_config.ignore_self_collisions_of_pair('arm_left_2_link', 'torso_fixed_column_link')
- #
- # self.collision_avoidance_config.ignore_self_collisions_of_pair('arm_left_2_link', 'torso_fixed_column_link')
+ self.set_default_visualization_marker_color(1, 1, 1, 0.7)
+ self.load_moveit_self_collision_matrix('package:https://tiago_dual_moveit_config/config/srdf/tiago.srdf')
+ self.overwrite_external_collision_avoidance('brumbrum',
+ number_of_repeller=2,
+ soft_threshold=0.2,
+ hard_threshold=0.1)
+ self.ignored_collisions = ['wheel_left_link',
+ 'wheel_right_link',
+ 'caster_back_left_2_link',
+ 'caster_back_right_2_link',
+ 'caster_front_left_2_link',
+ 'caster_front_right_2_link']
+ self.fix_joints_for_self_collision_avoidance(['head_1_joint',
+ 'head_2_joint',
+ 'gripper_left_left_finger_joint',
+ 'gripper_left_right_finger_joint',
+ 'gripper_right_left_finger_joint',
+ 'gripper_right_right_finger_joint'])
+ self.fix_joints_for_external_collision_avoidance(['gripper_left_left_finger_joint',
+ 'gripper_left_right_finger_joint',
+ 'gripper_right_left_finger_joint',
+ 'gripper_right_right_finger_joint'])
+ self.overwrite_external_collision_avoidance('arm_right_7_joint',
+ number_of_repeller=4,
+ soft_threshold=0.05,
+ hard_threshold=0.0,
+ max_velocity=0.2)
+ self.overwrite_external_collision_avoidance('arm_left_7_joint',
+ number_of_repeller=4,
+ soft_threshold=0.05,
+ hard_threshold=0.0,
+ max_velocity=0.2)
+ self.set_default_self_collision_avoidance(hard_threshold=0.04,
+ soft_threshold=0.08)
+ self.set_default_external_collision_avoidance(hard_threshold=0.03,
+ soft_threshold=0.08)
 
 
 class TiagoMujoco(TiagoBase):
@@ -120,25 +107,27 @@ def __init__(self):
  rotation_acceleration_limit=None)
 
 
-class TiagoStandAlone(TiagoBase):
+class Tiago_Standalone(TiagoBase):
  def __init__(self):
+ self.add_robot_from_parameter_server()
  super().__init__()
- self._general_config.control_mode = ControlModes.stand_alone
+ self.set_default_visualization_marker_color(1, 1, 1, 1)
+ self.set_control_mode(ControlModes.stand_alone)
+ self.publish_all_tf()
+ self.configure_VisualizationBehavior(in_planning_loop=True)
+ self.configure_CollisionMarker(in_planning_loop=True)
  self.root_link_name = 'map'
- # self.collision_avoidance_config.collision_checker = CollisionCheckerLib.none
- # self.disable_visualization()
- self.disable_tf_publishing()
  self.add_fixed_joint(parent_link='map', child_link='odom')
- self.add_robot_from_parameter_server('robot_description')
- self.register_controlled_joints(['torso_lift_joint', 'head_1_joint', 'head_2_joint'])
+ self.add_diff_drive_joint(parent_link_name='odom',
+ child_link_name='base_footprint',
+ name='brumbrum',
+ translation_velocity_limit=0.19,
+ rotation_velocity_limit=0.19)
+ self.register_controlled_joints(['torso_lift_joint', 'head_1_joint', 'head_2_joint', 'brumbrum'])
  self.register_controlled_joints(['arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint', 'arm_left_4_joint',
  'arm_left_5_joint', 'arm_left_6_joint', 'arm_left_7_joint'])
  self.register_controlled_joints(['arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint',
  'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint',
  'arm_right_7_joint'])
  self.register_controlled_joints(['gripper_right_left_finger_joint', 'gripper_right_right_finger_joint',
  'gripper_left_left_finger_joint', 'gripper_left_right_finger_joint'])
- self.add_diff_drive_interface(parent_link_name='odom',
- child_link_name='base_footprint',
- translation_velocity_limit=0.19,
- rotation_velocity_limit=0.19)

src/giskardpy/model/joints.py

@@ -1195,6 +1195,7 @@ class DiffDrive(Joint):
  def __init__(self,
  parent_link_name: my_string,
  child_link_name: my_string,
+ group_name: Optional[str] = None,
  name: Optional[my_string] = 'brumbrum',
  translation_velocity_limit: Optional[float] = 0.5,
  rotation_velocity_limit: Optional[float] = 0.6,
@@ -1208,17 +1209,18 @@ def __init__(self,
  x_vel_name: Optional[str] = 'base_footprint_x_vel',
  rot_vel_name: Optional[str] = 'base_footprint_rot_vel',
  **kwargs):
+ name = PrefixName(name, group_name)
  self.translation_velocity_limit = translation_velocity_limit
  self.rotation_velocity_limit = rotation_velocity_limit
  self.translation_acceleration_limit = translation_acceleration_limit
  self.rotation_acceleration_limit = rotation_acceleration_limit
  self.translation_jerk_limit = translation_jerk_limit
  self.rotation_jerk_limit = rotation_jerk_limit
- self.x_name = x_name
- self.y_name = y_name
- self.yaw_name = yaw_name
- self.x_vel_name = x_vel_name
- self.rot_vel_name = rot_vel_name
+ self.x_name = PrefixName(x_name, group_name)
+ self.y_name = PrefixName(y_name, group_name)
+ self.yaw_name = PrefixName(yaw_name, group_name)
+ self.x_vel_name = PrefixName(x_vel_name, group_name)
+ self.rot_vel_name = PrefixName(rot_vel_name, group_name)
  super().__init__(name, parent_link_name, child_link_name, w.eye(4))
 
  def create_free_variables(self):