Commit e08524f4 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by olivier stasse

[roscontrol_sot_tiago] Fix typos in joint names

parent c63cdcdf
......@@ -8,10 +8,10 @@ sot_controller:
- arm_left_5_joint
- arm_left_6_joint
- arm_left_7_joint
- gripper_joint
- gripper_finger_joint
- head_1_joint
- head_2_joint
- torso_joint
- torso_lift_joint
left_ft_sensor: left_ankle_ft
right_ft_sensor: right_ankle_ft
......
......@@ -8,7 +8,7 @@ sot_controller:
joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,
torques: torques, cmd-joints: control, cmd-effort: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
position_control_init_pos:
- name: torso_joint
- name: torso_lift_joint
des_pos: 0.0
- name: arm_1_joint
des_pos: 0.25847
......@@ -24,7 +24,7 @@ sot_controller:
des_pos: 0.0
- name: arm_7_joint
des_pos: 0.1
- name: gripper_joint
- name: gripper_finger_joint
des_pos: 0.034
- name: head_1_joint
des_pos: 0.0
......
......@@ -8,7 +8,7 @@ sot_controller:
joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,
torques: torques, cmd-joints: control, cmd-effort: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
position_control_init_pos:
- name: torso_joint
- name: torso_lift_joint
des_pos: 0.0
- name: arm_1_joint
des_pos: 0.25847
......
<launch>
<arg name="input_topic" default="/sot_hpp/state"/>
<arg name="output_prefix" default="/sot"/>
<arg name="output_prefix" default="sot"/>
<arg name="rviz" default="false"/>
<arg name="publish_root" default="true"/>
<node pkg="roscontrol_sot_tiago" name="$(anon republish)" type="republish" args="$(arg input_topic) $(arg output_prefix)/joint_state $(arg publish_root)"/>
<node pkg="tf" type="static_transform_publisher"
name="$(anon base_link_broadcaster)"
name="$(anon base_footprint_broadcaster)"
args="0 0 0 0 0 0 1 odom $(arg output_prefix)/odom 100"
if="$(arg publish_root)" />
<node pkg="tf" type="static_transform_publisher"
name="$(anon base_link_broadcaster)"
args="0 0 0 0 0 0 1 odom $(arg output_prefix)/base_link 100"
name="$(anon base_footprint_broadcaster)"
args="0 0 0 0 0 0 1 odom $(arg output_prefix)/base_footprint 100"
unless="$(arg publish_root)" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="$(anon rob_st_pub)" >
......
......@@ -29,7 +29,7 @@ if publish_root_wrt_odom:
output_prefix = output_topic.rsplit('/',1)[0] + '/'
else:
output_prefix = ""
rospy.loginfo("Will publish " + output_prefix + "base_link with respect to " + output_prefix + "odom")
rospy.loginfo("Will publish " + output_prefix + "base_footprint with respect to " + output_prefix + "odom")
jointnames = rospy.get_param("/sot_controller/joint_names")
......@@ -39,7 +39,7 @@ def jointreceived(jstates):
time = rospy.Time.now()
aJS.header.seq = seqnb
aJS.header.stamp = time
aJS.header.frame_id = "base_link"
aJS.header.frame_id = "base_footprint"
aJS.name = jointnames
aJS.position = jstates.data[6:]
aJS.velocity = []
......@@ -52,7 +52,7 @@ def jointreceived(jstates):
tf.transformations.quaternion_from_euler(
jstates.data[3], jstates.data[4], jstates.data[5]),
time,
output_prefix + "base_link",
output_prefix + "base_footprint",
output_prefix + "odom")
def listener():
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment