Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

possibility to get currents via status samples #30

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

possibility to get currents via status samples #30

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
76 changes: 38 additions & 38 deletions mars.orogen
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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')

Expand Down Expand Up @@ -141,7 +141,7 @@ end
task_context "DepthCamera" do
subclasses "CameraPlugin"
needs_configuration

output_port 'distance_image', ro_ptr('base::samples::DistanceImage')
end

Expand Down Expand Up @@ -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'
Expand All @@ -188,13 +188,15 @@ task_context "Joints" do
property('parallel_kinematics', 'std/vector<mars/ParallelKinematic>').
doc('maps a single joint command to two joints to control parallel kinematics simulated by two joints')
property('scaling', 'std/vector<double>').
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<double>').
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" ).
Expand All @@ -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</std/string>').
doc('Array of names of the ft sensors in the scene file.')
property('name_remap', 'std/vector</std/string>').
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
Expand Down Expand Up @@ -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")

Expand All @@ -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")

Expand All @@ -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<double>' ).
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).
Expand All @@ -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


Expand Down Expand Up @@ -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<double>')
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</base/Vector3d>')
doc 'Positions of the thrusters on the vehicle. The size of the vector must equal to amount_of_actuators'

Expand All @@ -394,7 +396,7 @@ task_context "ForceApplier" do

property('thruster_direction', 'std/vector</base/Vector3d>')
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
Expand Down Expand Up @@ -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<double>").
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


15 changes: 10 additions & 5 deletions tasks/Joints.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 );
Expand Down Expand Up @@ -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();

Expand All @@ -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<base::samples::RigidBodyState> rbs;
if( !_joint_transform.value().empty() && _transforms.connected() )
Expand Down Expand Up @@ -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() );
Expand Down Expand Up @@ -239,7 +244,7 @@ bool Joints::configureHook()
}


std::vector<std::string> rename = _name_remap.get();
std::vector<std::string> rename = _name_remap.get();
for( size_t i=0; i<mars_ids.size(); i++ )
{
if( !_scaling.value().empty() ){
Expand Down