Permalink
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
54 lines (41 sloc) 1.72 KB
<?xml version="1.0" encoding="UTF-8" ?>
<seatml>
<general name="SEED-Noid_Bridge_command_l">
<adaptor name="in" type="rtcin" datatype="TimedFloatSeq" />
<adaptor name="/larm_controller/command" type="ros_pub" datatype="trajectory_msgs/JointTrajectory" size="10"/>
</general>
<state name="main_state">
<rule source="in">
<script>
import rospy
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
data = seat.get_in_data()
Lval = seat.adaptors['/larm_controller/command'].newMessage()
Lval.joint_names.append("l_shoulder_p_joint")
Lval.joint_names.append("l_shoulder_r_joint")
Lval.joint_names.append("l_shoulder_y_joint")
Lval.joint_names.append("l_elbow_joint")
Lval.joint_names.append("l_wrist_y_joint")
Lval.joint_names.append("l_wrist_p_joint")
Lval.joint_names.append("l_wrist_r_joint")
Lval.joint_names.append("l_hand_y_joint")
Lp = JointTrajectoryPoint()
Lp.positions.append(-data.data[0]*3.14/180)
Lp.positions.append(data.data[1]*3.14/180)
Lp.positions.append(-data.data[2]*3.14/180)
Lp.positions.append(-data.data[3]*3.14/180)
Lp.positions.append(-data.data[4]*3.14/180)
Lp.positions.append(0)
Lp.positions.append(data.data[5]*3.14/180)
Lp.positions.append(-data.data[6]*3.14/180)
Lp.velocities.extend((0,0,0,0,0,0,0,0))
Lp.accelerations.extend((0,0,0,0,0,0,0,0))
Lp.effort.extend((0,0,0,0,0,0,0,0))
Lp.time_from_start = rospy.Duration(1)
Lval.points.append(Lp)
print("send: ", Lval)
seat.sendto("/larm_controller/command", Lval)
</script>
</rule>
</state>
</seatml>