How to set a joint position and and a joint force at the same time? Is it feasible with the current API?
Hi,
I am building a vehicle model with 2 revolute joints for each wheel. One joint for steering and one for spinning. I am working on the plugin that is attached to the model. When I set a position to the steering joint and a force to the spinning joint during the same OnUpdate event, the SetPosition seems to cancel out the resulting motion due to the Force I set on the other joint (spinning joint). I can't figure out why this is the case. Can anyone give me some more insight?
Thanks, Andrei
P.S. I am using Gazebo-7. I have added the model and (part of) the code from my plugin below.
Code sample:
void VehiclePlugin::OnUpdate(const common::UpdateInfo & /*_info*/)
{
update_counter = update_counter+1;
if ( update_counter>100 &&
update_counter<=300 ) {// appy a torque
double torque_joint = 50000;
SetWheelDriveTorque(WheelNames::CENTER_LEFT,torque_joint);
SetWheelDriveTorque(WheelNames::CENTER_RIGHT,torque_joint);
}
SetWheelSteerAngle(WheelNames::FRONT_LEFT,0.2);
SetWheelSteerAngle(WheelNames::FRONT_RIGHT,0.2);
SetWheelSteerAngle(WheelNames::CENTER_LEFT,0.2);
SetWheelSteerAngle(WheelNames::CENTER_RIGHT,0.2);
SetWheelSteerAngle(WheelNames::REAR_LEFT,0.2);
SetWheelSteerAngle(WheelNames::REAR_RIGHT,0.2);
(...)
}
void VehiclePlugin::SetWheelDriveTorque(const WheelNames & wheel_name, const double & torque_value)
{
// try to find wheel joint for this wheelname
if ( m_jointsptr_map.count(wheel_name)>0) {
physics::JointPtr wheel_joint_pointer = m_jointsptr_map[wheel_name];
wheel_joint_pointer->SetForce(1,torque_value);
}
}
void VehiclePlugin::SetWheelSteerAngle(const WheelNames & wheel_name, const double & angle_value)
{
// try to find wheel joint for this wheelname
if ( m_jointsptr_map.count(wheel_name)>0 ) {
physics::JointPtr wheel_joint_pointer = m_jointsptr_map[wheel_name];
if ( !wheel_joint_pointer->SetPosition(0,angle_value) ) {
std::cerr << ">>> WARNING: Could not set joint axis angle/position" << std::endl;
}
}
}
SDF model below (for a six-wheeled vehicle)
<model name="sixwheeled_vehicle">
<link name='lumped_chassis'>
<pose>0 0 5.16 0 0 0</pose>
<gravity>1</gravity>
<inertial>
<mass>54800</mass>
<inertia>
<ixx>308297.493333</ixx>
<iyy>417897.493333</iyy>
<izz>337933.333333</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
</inertial>
<visual name='lumped_chassis'>
<geometry>
<box>
<size>7 5 6.52</size>
</box>
</geometry>
</visual>
<collision name='collision_pseudo_chassis'>
<geometry>
<box>
<size>7 5 6.52</size>
</box>
</geometry>
</collision>
</link>
<link name="wheel_left_center">
<pose>0 2.266 0.76 -1.5707963267949 0 0</pose>
<gravity>1</gravity>
<inertial>
<mass>50</mass>
<inertia>
<ixx>7.88666666667</ixx>
<iyy>7.88666666667</iyy>
<izz>14.44</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
</inertial>
<collision name="collision_WLC">
<geometry>
<cylinder>
<radius>0.76</radius>
<length>0.4</length>
</cylinder>
</geometry>
<surface>
<friction>
<torsional>
<coefficient>0.0</coefficient>
<surface_radius>0.01</surface_radius>
<use_patch_radius>false</use_patch_radius>
</torsional>
<ode>
<fdir1>1 0 0</fdir1>
<mu>1.0</mu>
<mu2>0.0</mu2>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
</friction>
<contact>
<ode>
<min_depth>0.005</min_depth>
<kp>1e8</kp>
</ode>
</contact>
</surface>
</collision>
<visual name="visual_WLC">
<geometry>
<cylinder>
<radius>0.76</radius>
<length>0.4</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Black</name>
</script>
</material>
</visual>
</link>
<link name="wheel_left_front">
<pose>3.85 2.266 0.76 -1.5707963267949 0 0</pose>
<gravity>1</gravity>
<inertial>
<mass>50</mass>
<inertia>
<ixx>7.88666666667</ixx>
<iyy>7.88666666667</iyy>
<izz>14.44</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
</inertial>
<collision name="collision_WLF">
<geometry>
<cylinder>
<radius>0.76</radius>
<length>0.4</length>
</cylinder>
</geometry>
<surface>
<friction>
<torsional>
<coefficient>0.0</coefficient>
<surface_radius>0.01</surface_radius>
<use_patch_radius>false</use_patch_radius>
</torsional>
<ode>
<fdir1>1 0 0</fdir1>
<mu>1.0</mu>
<mu2>0.0</mu2>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
</friction>
<contact>
<ode>
<min_depth>0.005</min_depth>
<kp>1e8</kp>
</ode>
</contact>
</surface>
</collision>
<visual name="visual_WLF">
<geometry>
<cylinder>
<radius>0.76</radius>
<length>0.4</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Black</name>
</script>
</material>
</visual>
</link>
<link name="wheel_left_rear">
<pose>-3.85 2.266 0.76 -1.5707963267949 0 0</pose>
<gravity>1</gravity>
<inertial>
<mass>50</mass>
<inertia>
<ixx>7.88666666667</ixx>
<iyy>7.88666666667</iyy>
<izz>14.44</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
</inertial>
<collision name="collision_WLR">
<geometry>
<cylinder>
<radius>0.76</radius>
<length>0.4</length>
</cylinder>
</geometry>
<surface>
<friction>
<torsional>
<coefficient>0.0</coefficient>
<surface_radius>0.01</surface_radius>
<use_patch_radius>false</use_patch_radius>
</torsional>
<ode>
<fdir1>1 0 0</fdir1>
<mu>1.0</mu>
<mu2>0.0</mu2>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
</friction>
<contact>
<ode>
<min_depth>0.005</min_depth>
<kp>1e8</kp>
</ode>
</contact>
</surface>
</collision>
<visual name="visual_WLR">
<geometry>
<cylinder>
<radius>0.76</radius>
<length>0.4</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Black</name>
</script>
</material>
</visual>
</link>
<link name="wheel_right_center">
<pose>0 -2.266 0.76 -1.5707963267949 0 0</pose>
<gravity>1</gravity>
<inertial>
<mass>50</mass>
<inertia>
<ixx>7.88666666667</ixx>
<iyy>7.88666666667</iyy>
<izz>14.44</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
</inertial>
<collision name="collision_WRC">
<geometry>
<cylinder>
<radius>0.76</radius>
<length>0.4</length>
</cylinder>
</geometry>
<surface>
<friction>
<torsional>
<coefficient>0.0</coefficient>
<surface_radius>0.01</surface_radius>
<use_patch_radius>false</use_patch_radius>
</torsional>
<ode>
<fdir1>1 0 0</fdir1>
<mu>1.0</mu>
<mu2>0.0</mu2>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
</friction>
<contact>
<ode>
<min_depth>0.005</min_depth>
<kp>1e8</kp>
</ode>
</contact>
</surface>
</collision>
<visual name="visual_WRC">
<geometry>
<cylinder>
<radius>0.76</radius>
<length>0.4</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Black</name>
</script>
</material>
</visual>
</link>
<link name="wheel_right_front">
<pose>3.85 -2.266 0.76 -1.5707963267949 0 0</pose>
<gravity>1</gravity>
<inertial>
<mass>50</mass>
<inertia>
<ixx>7.88666666667</ixx>
<iyy>7.88666666667</iyy>
<izz>14.44</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
</inertial>
<collision name="collision_WRF">
<geometry>
<cylinder>
<radius>0.76</radius>
<length>0.4</length>
</cylinder>
</geometry>
<surface>
<friction>
<torsional>
<coefficient>0.0</coefficient>
<surface_radius>0.01</surface_radius>
<use_patch_radius>false</use_patch_radius>
</torsional>
<ode>
<fdir1>1 0 0</fdir1>
<mu>1.0</mu>
<mu2>0.0</mu2>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
</friction>
<contact>
<ode>
<min_depth>0.005</min_depth>
<kp>1e8</kp>
</ode>
</contact>
</surface>
</collision>
<visual name="visual_WRF">
<geometry>
<cylinder>
<radius>0.76</radius>
<length>0.4</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Black</name>
</script>
</material>
</visual>
</link>
<link name="wheel_right_rear">
<pose>-3.85 -2.266 0.76 -1.5707963267949 0 0</pose>
<gravity>1</gravity>
<inertial>
<mass>50</mass>
<inertia>
<ixx>7.88666666667</ixx>
<iyy>7.88666666667</iyy>
<izz>14.44</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
</inertial>
<collision name="collision_WRR">
<geometry>
<cylinder>
<radius>0.76</radius>
<length>0.4</length>
</cylinder>
</geometry>
<surface>
<friction>
<torsional>
<coefficient>0.0</coefficient>
<surface_radius>0.01</surface_radius>
<use_patch_radius>false</use_patch_radius>
</torsional>
<ode>
<fdir1>1 0 0</fdir1>
<mu>1.0</mu>
<mu2>0.0</mu2>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
</friction>
<contact>
<ode>
<min_depth>0.005</min_depth>
<kp>1e8</kp>
</ode>
</contact>
</surface>
</collision>
<visual name="visual_WRR">
<geometry>
<cylinder>
<radius>0.76</radius>
<length>0.4</length>
</cylinder>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Black</name>
</script>
</material>
</visual>
</link>
<joint name="WLC_DriveAndSteer_joint" type="universal">
<parent>lumped_chassis</parent>
<child>wheel_left_center</child>
<pose>0 0 -0.051 0 0 0</pose>
<axis>
<xyz>0 -1 0</xyz>
<limit>
<lower>-0.253072741539178</lower>
<upper>0.253072741539178</upper>
</limit>
</axis>
<axis2>
<dynamics>
<damping>0</damping>
<friction>0</friction>
</dynamics>
<xyz>0 0 1</xyz>
</axis2>
</joint>
<joint name="WLF_DriveAndSteer_joint" type="universal">
<parent>lumped_chassis</parent>
<child>wheel_left_front</child>
<pose>0 0 -0.051 0 0 0</pose>
<axis>
<xyz>0 -1 0</xyz>
<limit>
<lower>-1.08210413623648</lower>
<upper>0.872664625997165</upper>
</limit>
</axis>
<axis2>
<dynamics>
<damping>0</damping>
<friction>0</friction>
</dynamics>
<xyz>0 0 1</xyz>
</axis2>
</joint>
<joint name="WLR_DriveAndSteer_joint" type="universal">
<parent>lumped_chassis</parent>
<child>wheel_left_rear</child>
<pose>0 0 -0.051 0 0 0</pose>
<axis>
<xyz>0 -1 0</xyz>
<limit>
<lower>-0.872664625997165</lower>
<upper>1.08210413623648</upper>
</limit>
</axis>
<axis2>
<dynamics>
<damping>0</damping>
<friction>0</friction>
</dynamics>
<xyz>0 0 1</xyz>
</axis2>
</joint>
<joint name="WRC_DriveAndSteer_joint" type="universal">
<parent>lumped_chassis</parent>
<child>wheel_right_center</child>
<pose>0 0 -0.051 0 0 0</pose>
<axis>
<xyz>0 -1 0</xyz>
<limit>
<lower>-0.253072741539178</lower>
<upper>0.253072741539178</upper>
</limit>
</axis>
<axis2>
<dynamics>
<damping>0</damping>
<friction>0</friction>
</dynamics>
<xyz>0 0 1</xyz>
</axis2>
</joint>
<joint name="WRF_DriveAndSteer_joint" type="universal">
<parent>lumped_chassis</parent>
<child>wheel_right_front</child>
<pose>0 0 0.051 0 0 0</pose>
<axis>
<xyz>0 -1 0</xyz>
<limit>
<lower>-1.08210413623648</lower>
<upper>0.872664625997165</upper>
</limit>
</axis>
<axis2>
<dynamics>
<damping>0</damping>
<friction>0</friction>
</dynamics>
<xyz>0 0 1</xyz>
</axis2>
</joint>
<joint name="WRR_DriveAndSteer_joint" type="universal">
<parent>lumped_chassis</parent>
<child>wheel_right_rear</child>
<pose>0 0 -0.051 0 0 0</pose>
<axis>
<xyz>0 -1 0</xyz>
<limit>
<lower>-0.872664625997165</lower>
<upper>1.08210413623648</upper>
</limit>
</axis>
<axis2>
<dynamics>
<damping>0</damping>
<friction>0</friction>
</dynamics>
<xyz>0 0 1</xyz>
</axis2>
</joint>
<!--Plugin to use: path is relative to location where gzserver is started
Alternatively, path needs to be added to corresponding env variable-->
<plugin name="vehicleplugin" filename="build/libsixwheels_vehicle_move.so"/>
</model>
Asked by Andrei on 2016-11-04 09:39:39 UTC
Answers
Based on nkoenig's suggestion and an answer by AndreiHaidu to another question (see http://answers.gazebosim.org/question/2341/set-and-get-position-of-gazebo-model-using-ros-plugin/) my solution that seems to work so far is:
- created a PID object for each joint I want to control
- created an Update method for the controller (based on link above and the Update method in JointController.cc of Gazebo-7 source code) , code snippet given below
- finally, in the the OnUpdate step of my plugin class i set my desired steering angle set-point (m_setpoint) via a different function and call the Update method I implemented for all my joints' PIDs.
void PID_JointController::Update(){
common::Time currTime = this->m_model->GetWorld()->GetSimTime();
common::Time stepTime = currTime - this->m_prevUpdateTime;
this->m_prevUpdateTime = currTime;
if ( stepTime > 0 ) {
m_cmd_force = this->m_PositionPID.Update(
this->m_JointPtr->GetAngle(m_axis_index).Radian() -
this->m_setpoint, stepTime);
this->m_JointPtr->SetForce(m_axis_index, m_cmd_force);
}
Asked by Andrei on 2016-11-14 04:34:15 UTC
Comments
Can you post your code, model, and specify what version of gazebo you are using?
Asked by nkoenig on 2016-11-07 10:45:25 UTC
@nkoenig: I have edited my question with the SDF model and the code. If the code is not sufficient, let me know and I can send the entire file. (m_jointsptr_map is a map object containing mapping which is filled in at Load time. I have printed out the child link's velocities after the call to SetPosition and indeed the child gets 0 velocities after that call. Is there something I am doing wrong?
Asked by Andrei on 2016-11-08 04:01:44 UTC
In general, you don't want to use
SetPositionon a joint. TheSetPositionfunction bypasses the physics engine, and performs some teleportation of child links. You should apply forces to joints and write a controller to achieve desired joint angles.Asked by nkoenig on 2016-11-09 11:29:06 UTC
Ok. I see, thank you. I will try it with PID controller. I have seen an example here http://answers.gazebosim.org/question/2341/set-and-get-position-of-gazebo-model-using-ros-plugin/
Thanks again.
Asked by Andrei on 2016-11-09 12:54:56 UTC
@nkoenig: I guess I will have to write my own implementation of JointController::Update method for my steer axis controller of the universal joint, as from what I see in the Gazebo source code, the JointController:: Update method only updates the 0th axis of the joint and does not support an axis index.
Asked by Andrei on 2016-11-10 05:16:13 UTC