How to add disturbance in Joint space or Cartesian Space for Torque/Impedance Control

Typically: "How do I... ", "How can I... " questions
Post Reply
mohtashem
Posts: 15
Joined: 17 Apr 2017, 20:03

How to add disturbance in Joint space or Cartesian Space for Torque/Impedance Control

Post by mohtashem »

Hi,

I am working on KUKA LWR4+ robot. I've been trying to implement a control strategy involving Cartesian Impedance Control and Adaptive Fuzzy control. I am using (Eclipse)C++ and V-REP (Vrep-ROSinterface). Joints are in Torque/force mode with the motor enabled and the control loop disabled. I wanted to add sometime-varying disturbance (this disturbance has to be a torque or a force). The main idea is that Cartesian Impedance control is not enough to compensate for these disturbances, so we add an adaptive fuzzy controller for that purpose.

I think, I've successfully implemented Impedance control and the joint torques (obtained from Cartesian forces) are sent to the model in VREP using the following part of the code:

for (int i = 0; i < 7; i++)
{
torque_total(i) = torque_imp(i);
double sign = 1.0;
if(torque_total(i) < 0.0)
{
sign = -1.0;
}
joint_cmd_torque.velocity = sign*10000.0;
joint_cmd_torque.effort= fabs(torque_total(i));
}
joint_cmd_torque.name[0] = "joint_1";
joint_cmd_torque.name[1] = "joint_2";
joint_cmd_torque.name[2] = "joint_3";
joint_cmd_torque.name[3] = "joint_4";
joint_cmd_torque.name[4] = "joint_5";
joint_cmd_torque.name[5] = "joint_6";
joint_cmd_torque.name[6] = "joint_7";
joint_torque_pub.publish(joint_cmd_torque);

Then, I tried adding noise_term(i) = 100*sin(X*ts) to the system as follows:

torque_total(i) = torque_imp(i)+ noise_term(i);
Rest of the code is the same.
But the robot tracks the desired trajectory as perfectly as Impedance alone. The addition of torque terms due to the adaptive fuzzy controller seems results in poor tracking of the trajectory.
So I wanted to ask:

1) Am I adding the disturbance correctly?
2) The impedance control alone seems ideally perfect. Is it always the case in V-REP. How can I try to simulate it to make it more realistic?

Kindly, advise on the issue.
Best Regards,

coppelia
Site Admin
Posts: 10339
Joined: 14 Dec 2012, 00:25

Re: How to add disturbance in Joint space or Cartesian Space for Torque/Impedance Control

Post by coppelia »

Hello,

I am afraid we have not enough experience with impedance control algorithms. But on the question why disturbance does not have any effect, my only guess would be that the disturbance amplitude is probably not large enough (or frequency too high?).

At the same time, in order to get best results, you should make sure that your control happens in every step of a dynamic calculation pass. By default, there are 10 dyn. calculation passes for a single simulation step (i.e. by default, the dyn. calculation step is 5ms, the simulation step is 50ms. So make sure to set the simulation step to 5ms, and make sure that the simulation runs in synchronized mode. For that, have a look at the model models/tools/rosInterface helper tool.ttm.

Cheers

mohtashem
Posts: 15
Joined: 17 Apr 2017, 20:03

Re: How to add disturbance in Joint space or Cartesian Space for Torque/Impedance Control

Post by mohtashem »

Hi,

Thank you for your suggestion. As a matter of fact, I am using rosinterface helper in my scene to obtain the simulation time which I use in C++ to send the trajectory to the robot model in V-Rep. So when I run the code in ROS, it automatically starts starts the simulation in V-Rep. I just have a couple of doubts about it. I was wondering if you could clarify it for me.

My simulation step in V-rep is set to 5ms. In C++ I am using

ros::Rate loop_rate(200); // in Hz for samplingTime = 5ms

Am I correct in assuming so?

What about the number of messages in whilst publishing to or subscribing from a topic ? For instance

n.advertise<std_msgs::Float32MultiArray>("cartesian_measured",1000); // 1000 equals the number of messages in buffer
ros::Subscriber euler_pub = n.subscribe("Euler",1,euler_cb); // 1 message in buffer

Do they have any effect on the simulation?

Thanks

coppelia
Site Admin
Posts: 10339
Joined: 14 Dec 2012, 00:25

Re: How to add disturbance in Joint space or Cartesian Space for Torque/Impedance Control

Post by coppelia »

Make sure the synchronized mode is enabled. Then, start simulation by posting to startSimulation. Then:
  • Publish data that V-REP subscribed to. Make sure that subscribers have a queue of 1.
  • post to triggerNextStep. One simulation step (i.e. by default 50ms of simulation time) will be executed.
  • Wait to receive simulationStepDone. Then continue with point 1 above.
Cheers

mohtashem
Posts: 15
Joined: 17 Apr 2017, 20:03

Re: How to add disturbance in Joint space or Cartesian Space for Torque/Impedance Control

Post by mohtashem »

Hi, thanks for your reply.
I think I have done exactly what has been mentioned in the last post.

Is there a proper way of checking if synchronous mode is working fine?

Although, when i run my simulation via ros, the model in my v-rep starts functioning and maintains the position error does converge to zero in torque control mode. However, I am not sure as to is the synchronous mode working properly.

I ran "rosbag record" to record the data from ROS. Then On using "rosbag info"
i display the information about the simulation session:
-----------------------------------------------------------------------------------------
rosbag info 2017-11-28-10-48-29.bag
path: 2017-11-28-10-48-29.bag
version: 2.0
duration: 1:46s (106s)
start: Nov 28 2017 10:48:29.07 (1511862509.07)
end: Nov 28 2017 10:50:15.35 (1511862615.35)
size: 12.1 MB
messages: 120122
compression: none [15/15 chunks]
types: rosgraph_msgs/Log [acffd30cd6b6de30f120938c17c593fb]
sensor_msgs/JointState [3066dcd76a6cfaef579bd0f34173e9fd]
std_msgs/Bool [8b94c1b53db61fb6aed406028ad6332a]
std_msgs/Float32 [73fcbf46b49191e672908e50842a83d4]
std_msgs/Float32MultiArray [6a40e0ffa6a17a503ac3f8616991b1f6]
std_msgs/Int32 [da5909fbe378aeaf85e547e830cc1bb7]
topics: /Euler 1135 msgs : std_msgs/Float32MultiArray
/cartesian_measured 8067 msgs : std_msgs/Float32MultiArray
/cartesiantorque 8046 msgs : std_msgs/Float32MultiArray
/enableSyncMode 8067 msgs : std_msgs/Bool
/error 8066 msgs : std_msgs/Float32MultiArray
/fuzzycartesiantorque 8046 msgs : std_msgs/Float32MultiArray
/getJointState 1135 msgs : sensor_msgs/JointState
/noise_term 8066 msgs : std_msgs/Float32MultiArray
/privateMsgAux 1136 msgs : std_msgs/Bool
/rosout 23 msgs : rosgraph_msgs/Log
/rosout_agg 18 msgs : rosgraph_msgs/Log
/setjointforce 8046 msgs : sensor_msgs/JointState
/simulationState 1637 msgs : std_msgs/Int32
/simulationStepDone 1136 msgs : std_msgs/Bool
/simulationTime 1136 msgs : std_msgs/Float32
/startSimulation 8046 msgs : std_msgs/Bool
/theta_x 8046 msgs : std_msgs/Float32MultiArray
/theta_y 8046 msgs : std_msgs/Float32MultiArray
/theta_z 8046 msgs : std_msgs/Float32MultiArray
/triggerNextStep 8066 msgs : std_msgs/Bool
/x_des 8046 msgs : std_msgs/Float32MultiArray
/xdot_msr 8066 msgs : std_msgs/Float32MultiArray
---------------------------------------------------------------------------------------------
The number of messages published by V-rep topics(1136 msgs) is lower than the number of messages published from C++ topics( 8046 msgs ). I was thinking that, using synchrounous mode, i should have an equal number of messages published from both nodes, as I am using them both together in some equations.

Any idea how to fix it?
Best Regards

coppelia
Site Admin
Posts: 10339
Joined: 14 Dec 2012, 00:25

Re: How to add disturbance in Joint space or Cartesian Space for Torque/Impedance Control

Post by coppelia »

Not sure how you enable the synchronous mode, probably before starting the simulation. You can try to enable it while simulation is running, then you should notice a difference (but the first method is better). Also, not running in synchronous mode will make things faster, so this should also give you a clue as to whether you are really running in sync mode.

But the sync mode does not guarantee the same amount of messages from V-REP or C/C++ (you can run in sync mode and still decide to publish from C/C++ twice as many messages as you receive from V-REP). Also, as long as the next step is not triggered, the ROS message pump in V-REP will still run, but the scripts won't (unless for callback functions triggered by ROS messages).

Cheers

Post Reply