Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
talos-metapkg-ros-control-sot
Commits
abdb87a0
Unverified
Commit
abdb87a0
authored
Jul 06, 2018
by
olivier stasse
Committed by
GitHub
Jul 06, 2018
Browse files
Merge pull request #2 from jmirabel/master
Update republish + config for position control
parents
d464dabc
f8db9af3
Changes
4
Hide whitespace changes
Inline
Side-by-side
roscontrol_sot_talos/config/sot_talos_params.yaml
View file @
abdb87a0
...
...
@@ -7,8 +7,8 @@ sot_controller:
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
,
gripper_right_joint
,
head_1_joint
,
head_2_joint
]
map_rc_to_sot_device
:
{
motor-angles
:
motor-angles
,
joint-angles
:
joint-angles
,
velocities
:
velocities
,
torques
:
torques
,
cmd-joints
:
control
,
cmd-effort
:
control
}
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
:
leg_left_1_joint
des_pos
:
0.0
...
...
roscontrol_sot_talos/config/sot_talos_params_gazebo.yaml
View file @
abdb87a0
...
...
@@ -8,8 +8,8 @@ sot_controller:
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
,
gripper_right_joint
,
head_1_joint
,
head_2_joint
]
map_rc_to_sot_device
:
{
motor-angles
:
motor-angles
,
joint-angles
:
joint-angles
,
velocities
:
velocities
,
torques
:
torques
,
cmd-joints
:
control
,
cmd-effort
:
control
}
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
:
leg_left_1_joint
des_pos
:
0.0
...
...
roscontrol_sot_talos/launch/display_sot.launch
View file @
abdb87a0
...
...
@@ -2,11 +2,11 @@
<arg name="input_topic" default="/sot_hpp/state"/>
<arg name="output_prefix" default="/sot"/>
<node pkg="roscontrol_sot_talos" name="republish" type="republish" args="$(arg input_topic) $(arg output_prefix)/joint_state"/>
<node pkg="roscontrol_sot_talos" name="republish" type="republish" args="$(arg input_topic) $(arg output_prefix)/joint_state
true
"/>
<node pkg="tf" type="static_transform_publisher"
name="base_link_broadcaster"
args="0 0 0 0 0 0 1
base_link
$(arg output_prefix)/
base_link
100" />
args="0 0 0 0 0 0 1
odom
$(arg output_prefix)/
odom
100" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub" >
<remap from="/joint_states" to="$(arg output_prefix)/joint_state" />
...
...
roscontrol_sot_talos/scripts/republish
View file @
abdb87a0
#!/usr/bin/env python
# license removed for brevity
import
rospy
import
rospy
,
tf
from
dynamic_graph_bridge_msgs.msg
import
Vector
from
sensor_msgs.msg
import
JointState
import
sys
# Arg 1
input_topic
=
"/sot_hpp/state"
# Arg 2
output_topic
=
"/sot/joint_state"
# Arg 3
publish_root_wrt_odom
=
False
if
len
(
sys
.
argv
)
>
1
:
input_topic
=
sys
.
argv
[
1
]
if
len
(
sys
.
argv
)
>
2
:
output_topic
=
sys
.
argv
[
2
]
else
:
output_topic
=
"/sot/joint_state"
else
:
input_topic
=
"/sot_hpp/state"
if
len
(
sys
.
argv
)
>
3
:
publish_root_wrt_odom
=
(
sys
.
argv
[
3
].
lower
()
in
(
"true"
,
"on"
))
rospy
.
init_node
(
'sot_reemitter'
,
anonymous
=
True
)
pub
=
rospy
.
Publisher
(
output_topic
,
JointState
,
queue_size
=
10
)
...
...
@@ -20,13 +24,21 @@ pub = rospy.Publisher(output_topic, JointState, queue_size=10)
seqnb
=
0
aJS
=
JointState
()
if
publish_root_wrt_odom
:
if
output_topic
.
find
(
'/'
)
>=
0
:
output_prefix
=
output_topic
.
rsplit
(
'/'
,
1
)[
0
]
+
'/'
else
:
output_prefix
=
""
rospy
.
loginfo
(
"Will publish "
+
output_prefix
+
"base_link with respect to "
+
output_prefix
+
"odom"
)
jointnames
=
rospy
.
get_param
(
"/sot_controller/joint_names"
)
def
jointreceived
(
jstates
):
global
seqnb
seqnb
=
seqnb
+
1
time
=
rospy
.
Time
.
now
()
aJS
.
header
.
seq
=
seqnb
aJS
.
header
.
stamp
=
rospy
.
Time
.
now
()
aJS
.
header
.
stamp
=
time
aJS
.
header
.
frame_id
=
"base_link"
aJS
.
name
=
jointnames
aJS
.
position
=
jstates
.
data
[
6
:]
...
...
@@ -34,6 +46,15 @@ def jointreceived(jstates):
aJS
.
effort
=
[]
pub
.
publish
(
aJS
)
if
publish_root_wrt_odom
and
len
(
jstates
.
data
)
>
6
:
br
=
tf
.
TransformBroadcaster
()
br
.
sendTransform
(
jstates
.
data
[
0
:
3
],
tf
.
transformations
.
quaternion_from_euler
(
jstates
.
data
[
3
],
jstates
.
data
[
4
],
jstates
.
data
[
5
]),
time
,
output_prefix
+
"base_link"
,
output_prefix
+
"odom"
)
def
listener
():
rospy
.
Subscriber
(
input_topic
,
Vector
,
jointreceived
)
rospy
.
spin
()
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment