From 14f48febb8d88e4ea279481b9864bab9c920d3f1 Mon Sep 17 00:00:00 2001 From: dmronga Date: Sun, 2 Aug 2020 17:16:20 +0200 Subject: [PATCH] Fix test scripts --- scripts/RMLCartesianPositionTask.rb | 21 ++++++++++----------- scripts/RMLCartesianVelocityTask.rb | 19 +++++++++---------- 2 files changed, 19 insertions(+), 21 deletions(-) diff --git a/scripts/RMLCartesianPositionTask.rb b/scripts/RMLCartesianPositionTask.rb index e060443..36e403f 100644 --- 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 index 810bafc..c20ebcd 100644 --- 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