Fix test scripts

thorounibremen · Aug 2, 2020 · 14f48fe · 14f48fe
1 parent 17ec6cb
commit 14f48fe
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 21 deletions.
diff --git a/scripts/RMLCartesianPositionTask.rb b/scripts/RMLCartesianPositionTask.rb
@@ -11,12 +11,11 @@
     interpolator.configure
     interpolator.start
 
-    cartesian_state                  = Types.base.samples.CartesianState.new
+    cartesian_state                  = Types.base.samples.RigidBodyStateSE3.new
     cartesian_state.pose.position    = Types.base.Vector3d.new(0,0,0)
     cartesian_state.pose.orientation = Types.base.Quaterniond.from_euler(Types.base.Vector3d.new(-Math::PI,-Math::PI/2,Math::PI/2),2,1,0)
     cartesian_state.time             = Types.base.Time.now
-    cartesian_state.source_frame     = "current_state_tip"
-    cartesian_state.target_frame     = "current_state_root"
+    cartesian_state.frame_id        = "current_state_root"
 
     Readline.readline("Press Enter to start")
 
@@ -25,12 +24,12 @@
 
     Readline.readline("Press Enter to send target")
 
-    target                  = Types.base.samples.CartesianState.new
-    target.pose.position    = Types.base.Vector3d.new(1,2,3)
-    target.pose.orientation = Types.base.Quaterniond.from_euler(Types.base.Vector3d.new(Math::PI,Math::PI/2,-Math::PI/2),2,1,0)
-    target.time             = Types.base.Time.now
-    target.source_frame     = "target_tip"
-    target.target_frame     = "target_root"
+    target             = Types.base.samples.RigidBodyState.new
+    target.position    = Types.base.Vector3d.new(1,2,3)
+    target.orientation = Types.base.Quaterniond.from_euler(Types.base.Vector3d.new(Math::PI,Math::PI/2,-Math::PI/2),2,1,0)
+    target.time        = Types.base.Time.now
+    target.sourceFrame = "target_tip"
+    target.targetFrame = "target_root"
 
     target_writer = interpolator.target.writer
     target_writer.write(target)
@@ -39,9 +38,9 @@
     while true
         command = command_reader.read
         if command
-            puts "Target position: "    + target.pose.position[0].to_s  + " " + target.pose.position[1].to_s  + " " + target.pose.position[2].to_s
+            puts "Target position: "    + target.position[0].to_s  + " " + target.position[1].to_s  + " " + target.position[2].to_s
             puts "Commanded position: " + command.pose.position[0].to_s + " " + command.pose.position[1].to_s + " " + command.pose.position[2].to_s
-            euler = target.pose.orientation.to_euler
+            euler = target.orientation.to_euler
             puts "Target orientation: "    + euler[0].to_s  + " " + euler[1].to_s  + " " + euler[2].to_s
             euler = command.pose.orientation.to_euler
             puts "Commanded orientation: " + euler[0].to_s  + " " + euler[1].to_s  + " " + euler[2].to_s

diff --git a/scripts/RMLCartesianVelocityTask.rb b/scripts/RMLCartesianVelocityTask.rb
@@ -11,12 +11,11 @@
     interpolator.configure
     interpolator.start
 
-    cartesian_state                  = Types.base.samples.CartesianState.new
+    cartesian_state                  = Types.base.samples.RigidBodyStateSE3.new
     cartesian_state.pose.position    = Types.base.Vector3d.new(0,0,0)
     cartesian_state.pose.orientation = Types.base.Quaterniond.from_euler(Types.base.Vector3d.new(-Math::PI,-Math::PI/2,Math::PI/2),2,1,0)
     cartesian_state.time             = Types.base.Time.now
-    cartesian_state.source_frame     = "current_state_tip"
-    cartesian_state.target_frame     = "current_state_root"
+    cartesian_state.frame_id         = "current_state_tip"
 
     Readline.readline("Press Enter to start")
 
@@ -25,12 +24,12 @@
 
     Readline.readline("Press Enter to send target")
 
-    target                  = Types.base.samples.CartesianState.new
-    target.twist.linear         = Types.base.Vector3d.new(1,2,3)
-    target.twist.angular = Types.base.Vector3d.new(Math::PI,Math::PI/2,-Math::PI/2)
+    target                  = Types.base.samples.RigidBodyState.new
+    target.velocity         = Types.base.Vector3d.new(1,2,3)
+    target.angular_velocity = Types.base.Vector3d.new(Math::PI,Math::PI/2,-Math::PI/2)
     target.time             = Types.base.Time.now
-    target.source_frame      = "target_tip"
-    target.target_frame      = "target_root"
+    target.sourceFrame      = "target_tip"
+    target.targetFrame      = "target_root"
 
     target_writer = interpolator.target.writer
     target_writer.write(target)
@@ -39,9 +38,9 @@
     while true
         command = command_reader.read
         if command
-            puts "Target twist.linear: "    + target.twist.linear[0].to_s  + " " + target.twist.linear[1].to_s  + " " + target.twist.linear[2].to_s
+            puts "Target twist.linear: "    + target.velocity[0].to_s  + " " + target.velocity[1].to_s  + " " + target.velocity[2].to_s
             puts "Commanded twist.linear: " + command.twist.linear[0].to_s + " " + command.twist.linear[1].to_s + " " + command.twist.linear[2].to_s
-            puts "Target angular twist.linear: "    + target.twist.angular[0].to_s  + " " + target.twist.angular[1].to_s  + " " + target.twist.angular[2].to_s
+            puts "Target angular twist.linear: "    + target.angular_velocity[0].to_s  + " " + target.angular_velocity[1].to_s  + " " + target.angular_velocity[2].to_s
             puts "Commanded angular twist.linear: " + command.twist.angular[0].to_s + " " + command.twist.angular[1].to_s + " " + command.twist.angular[2].to_s
             puts "---------------------------------------------------"
         end