From 7c4fb05c3cbfc641c7964b5497417a8fbadb0cc9 Mon Sep 17 00:00:00 2001 From: Alexander Dettmann Date: Wed, 7 Mar 2018 21:00:26 +0100 Subject: [PATCH] possibility to get currents via status samples --- mars.orogen | 76 ++++++++++++++++++++++++------------------------ tasks/Joints.cpp | 15 ++++++---- 2 files changed, 48 insertions(+), 43 deletions(-) diff --git a/mars.orogen b/mars.orogen index eb90ef97..3cb5e97e 100644 --- a/mars.orogen +++ b/mars.orogen @@ -25,7 +25,7 @@ import_types_from "base" # subclasses 'mars::Task' # end # -# Pay attention that all these tasks have to be started within +# Pay attention that all these tasks have to be started within # one deployment to be able to share their static resources. task_context "Task" do needs_configuration @@ -107,7 +107,7 @@ task_context "Task" do #This operation moves a node to a specific position, simpliar to the positions property but can be used during runtime operation("move_node"). argument("arg","/mars/Positions") - + output_port('time', 'double') output_port('simulated_time', '/base/Time') @@ -141,7 +141,7 @@ end task_context "DepthCamera" do subclasses "CameraPlugin" needs_configuration - + output_port 'distance_image', ro_ptr('base::samples::DistanceImage') end @@ -172,7 +172,7 @@ task_context "HighResRangeFinder" do doc('Adds another camera which will be used for pointcloud creation'). returns("bool"). argument('name', 'std::string', "Name of the camera within the scene file"). - argument('orientation', 'double', "To get a full 360 degree view you have to add four 90-cameras with 0, 90, 180 and 270") + argument('orientation', 'double', "To get a full 360 degree view you have to add four 90-cameras with 0, 90, 180 and 270") output_port('pointcloud', '/base/samples/Pointcloud'). doc 'Created by using the distance image of the DepthCamera' @@ -188,13 +188,15 @@ task_context "Joints" do property('parallel_kinematics', 'std/vector'). doc('maps a single joint command to two joints to control parallel kinematics simulated by two joints') property('scaling', 'std/vector'). - doc('Optional array of scale values that are applied to the mars joint values. Needs to be empty or the same size as names.') + doc('Optional array of scale values that are applied to the mars joint values. Needs to be empty or the same size as names.') property('offset', 'std/vector'). - doc('Optional array of offset values that added to the scaled mars joint values. Needs to be empty or the same size as names.') - # optional configuration for the rigid_body_states - # this module will generate on the + doc('Optional array of offset values that added to the scaled mars joint values. Needs to be empty or the same size as names.') + property('use_currents_as_effort', 'bool', false). + doc('If true, currents are send as effort whithin the status sample (instead of torques)') + # optional configuration for the rigid_body_states + # this module will generate on the property "joint_transform", "base/JointTransformVector" - + property("controlMode", "/mars/JointPositionAndSpeedControlMode", :IGNORE).doc('sets the how speed is interpreted in case speed and position is set :MAX_SPEED :IGNORE. unsupported: :SPEED_AT_POS') output_port( "status_samples", "base/samples/Joints" ). @@ -214,27 +216,27 @@ task_context "Joints" do end task_context "ForceTorque6DOF" do - + subclasses "Plugin" - + output_port("wrenches_deprecated", "std/vector< /base/samples/Wrench >") output_port("wrenches", "/base/samples/Wrenches") - + property('names', 'std/vector'). doc('Array of names of the ft sensors in the scene file.') property('name_remap', 'std/vector'). doc('If this array is set the sensors will be renamed for the external interface') - + end # This plugin can be used to "trigger" a simlation cycle, -# it can be useful for operating mars with more-than-realtime +# it can be useful for operating mars with more-than-realtime # mode and use the task_scheduler for a fixed execution sequence task_context "Trigger" do subclasses "Plugin" property "do_step","bool",false - doc("This components triggeres the mars simulation cycle if the update-hook is called, + doc("This components triggeres the mars simulation cycle if the update-hook is called, this is useful if an sequence between componentes is needed, like it is possible with the trigger_component") end @@ -274,10 +276,10 @@ task_context "RotatingLaserRangeFinder" do property('name', '/std/string', 'unknown_ray_sensor'). doc('Name of the RotatingRaySensor in the scene file') - + property('min_range', 'double', 0.0). doc("Minimum valid range of the laser range finder") - + property('max_range', 'double', 20.0). doc("Maximum valid range of the laser range finder") @@ -287,7 +289,7 @@ end task_context "IMU" do subclasses "Plugin" - + property("provide_position", "/bool",false). doc("Set to true is this Sensor should output position readings") @@ -303,7 +305,7 @@ task_context "IMU" do property("world_frame", "std/string", "world"). doc "The name of the world frame." property('rotate_node_relative', '/std/vector' ). - doc('if the robot is moved my mars (yaml), rotation of the IMU may be wrong and can be set here (in degrees)') + doc('if the robot is moved my mars (yaml), rotation of the IMU may be wrong and can be set here (in degrees)') property("position_sigma", "double", 0.0). doc "Standard deviation of the position, that will be applied to the measurement." property("orientation_sigma", "double", 0.0). @@ -317,18 +319,18 @@ task_context "IMU" do doc 'provides timestamped IMUReading samples containing the orientation estimate as reported by the IMU.' output_port('calibrated_sensors', '/base/samples/IMUSensors'). doc 'provides timestamped IMUReading samples containing the calibrated sensor readings.' - + needs_configuration - + property('name', '/std/string', 'imu'). doc('name of the node in the scene file from which to get the imu data') - + operation('estimate_bias'). argument('duration', '/uint16_t'). returns('/bool'). - doc("Normally triggers bias estimation on the xsens imu, in simulation thios does nothing") + doc("Normally triggers bias estimation on the xsens imu, in simulation thios does nothing") + - end @@ -376,16 +378,16 @@ end task_context "ForceApplier" do subclasses "Plugin" needs_configuration - + property('node_name', 'std/string') doc 'the name of the vehicle in the scene file' - + property('amount_of_actuators', 'int') doc 'the amount of actuators the vehicle has' - + property('maximum_thruster_force', 'std/vector') doc 'The maximum thruster foce for each actuator. Is also used as a factor. The size of the vector must equal to amount_of_actuators' - + property('thruster_position', 'std/vector') doc 'Positions of the thrusters on the vehicle. The size of the vector must equal to amount_of_actuators' @@ -394,7 +396,7 @@ task_context "ForceApplier" do property('thruster_direction', 'std/vector') doc 'Directions of the thruster force on the vehicle. The size of the vector must equal to amount_of_actuators' - + input_port("command", "base/commands/Joints"). doc("actuator command as joint type"). needs_buffered_connection @@ -431,25 +433,23 @@ task_context "AuvMotion" do For more information about the model, see: Fossen, T.I.: Guidance and control of ocean vehicles") subclasses "ForceApplier" - + property("linear_damp", "/base/Vector6d"). - doc("Linear damping coefficients, in 6 degrees of freedom") - + doc("Linear damping coefficients, in 6 degrees of freedom") + property("square_damp", "/base/Vector6d"). doc("Square damping coefficients, in 6 degrees of freedom") - + property("thruster_coefficients" , "/std/vector"). doc("The square coefficients of the thrusters, the size of the list must be the amount of actuators") - + property("voltage", "double", 0). doc("Maximum volatage of the thrusters, used for pwm to force calculation") - + property("cob", "/base/Vector3d"). doc("Center of Bouyancy, with respect to the origin of the vehicle") - + property("buoyancy_force", "double", 0). doc("Buoyancy force applied to the vehicle") end - - diff --git a/tasks/Joints.cpp b/tasks/Joints.cpp index 1ff2abb5..dd628126 100644 --- a/tasks/Joints.cpp +++ b/tasks/Joints.cpp @@ -70,7 +70,7 @@ void Joints::update(double delta_t) if (it == cmd.names.end()){ continue; } - + base::JointState &curCmd(cmd[*it]); mars::sim::SimMotor *motor = control->motors->getSimMotor( conv.mars_id ); @@ -130,7 +130,12 @@ void Joints::update(double delta_t) state.position = conv->fromMars(conv->updateAbsolutePosition( motor->getActualPosition() )); state.speed = motor->getJoint()->getVelocity() * conv->scaling; - state.effort = conv->fromMars( motor->getTorque() ); + + if (_use_currents_as_effort.get()) { + state.effort = conv->fromMars( motor->getCurrent() ); + } else { + state.effort = conv->fromMars( motor->getTorque() ); + } currents[conv->externalName] = motor->getCurrent(); @@ -155,7 +160,7 @@ void Joints::update(double delta_t) currents.time = status.time; _current_values.write(currents); - // see if we have configuration for the joint_transforms + // see if we have configuration for the joint_transforms // and the output port for it is connected std::vector rbs; if( !_joint_transform.value().empty() && _transforms.connected() ) @@ -190,7 +195,7 @@ bool Joints::configureHook() mars_ids.clear(); - // fill the joint structure + // fill the joint structure mars_ids.resize( num_joints ); cmd.resize( num_joints); status.resize( num_joints - parallel_kinematics.size() ); @@ -239,7 +244,7 @@ bool Joints::configureHook() } - std::vector rename = _name_remap.get(); + std::vector rename = _name_remap.get(); for( size_t i=0; i