Gravity compensation problem of force sensor in CoppeliaSim
-
- Posts: 6
- Joined: 20 Mar 2023, 14:50
Gravity compensation problem of force sensor in CoppeliaSim
I built the model system of robot arm, force sensor and gripper in CoppeliaSim. How do I calibrate and gravimetrically compensate the force sensor in the simulation so that the force measured by the force sensor with the gripper is equal to the external torque applied to it? Thanks.
Re: Gravity compensation problem of force sensor in CoppeliaSim
So you have a force-sensor between your arm and your gripper?
It seems you want to subtract the force caused by gripper's weight from the force-sensor's reading, is it correct?
It seems you want to subtract the force caused by gripper's weight from the force-sensor's reading, is it correct?
-
- Posts: 6
- Joined: 20 Mar 2023, 14:50
Re: Gravity compensation problem of force sensor in CoppeliaSim
Hello ferri,
Thank you for your reply.
As you said, I have a force sensor between my arm and the hand gripper. I want to subtract the force from the gripper weight from the force sensor reading. I tried to do gravity compensation, but the sensor does not show zero after compensation. I wonder if you have a corresponding solution?
Looking forward to your reply.
Best regards,
Robot_XiaoXi
Thank you for your reply.
As you said, I have a force sensor between my arm and the hand gripper. I want to subtract the force from the gripper weight from the force sensor reading. I tried to do gravity compensation, but the sensor does not show zero after compensation. I wonder if you have a corresponding solution?
Looking forward to your reply.
Best regards,
Robot_XiaoXi
-
- Posts: 6
- Joined: 20 Mar 2023, 14:50
Re: Gravity compensation problem of force sensor in CoppeliaSim
Hello ferri,
Thank you for your reply.
As you said, I have a force sensor between my arm and the hand gripper. I want to subtract the force caused by gripper's weight from the force-sensor's reading. I tried to do gravity compensation, but the sensor does not show zero after compensation. I wonder if you have a corresponding solution?
Looking forward to your reply.
Best regards,
Robot_XiaoXi
Re: Gravity compensation problem of force sensor in CoppeliaSim
To subtract the torque/force of the gripper you have two possibilities:
- manually compute gripper's center of mass, total mass \(m\), then express gripper's center of mass \(\vec{r}\) and gravity vector \(\vec{g}\) in force sensor's reference frame, and compute resulting force/torque (remember that torque \(\vec\tau=\vec{r} \times \left(m \vec{g}\right)\))
- add an anti-gravity force to the gripper (sim.addForce)
Re: Gravity compensation problem of force sensor in CoppeliaSim
For the record, to implement first proposed approach, you would first compute a list of dynamic shapes that you want to exclude from force sensor's reading, for which you'll be computing force and torque manually:
then compute mass and center of mass of those shapes (in absolute coords):
then convert into local coords (sensor's):
and finally compute force and torque caused by those shapes:
Please note that everything's said is only valid for a static configuration. If the robot moves, there are other accelerations that are not taken into account.
Code: Select all
allNonStaticShapes={}
-- add items to 'allNonStaticShapes'
Code: Select all
com,mass=Vector{0,0,0},0
for i,shape in ipairs(allNonStaticShapes) do
local mass1=sim.getShapeMass(shape)
local inertia,comFrame=sim.getShapeInertia(shape)
local shapeFrame=sim.getObjectMatrix(shape,sim.handle_world)
local m=sim.multiplyMatrices(shapeFrame,comFrame)
com=com+mass1*Vector{m[4],m[8],m[12]}
mass=mass+mass1
end
if mass~=0 then
com=com/mass
end
Code: Select all
minv=sim.getObjectMatrix(sensorHandle,sim.handle_world);sim.invertMatrix(minv)
minv=Matrix4x4(minv)
com=minv:mult(com)
minv[1][4]=0
minv[2][4]=0
minv[3][4]=0
g=minv:mult(Vector{0,0,-9.81})
Code: Select all
force=g*mass
torque=com:cross(force)
force
and torque
are also in sensor's reference frame.Please note that everything's said is only valid for a static configuration. If the robot moves, there are other accelerations that are not taken into account.
-
- Posts: 6
- Joined: 20 Mar 2023, 14:50
Re: Gravity compensation problem of force sensor in CoppeliaSim
hello fferri,fferri wrote: ↑22 Mar 2023, 15:14 To subtract the torque/force of the gripper you have two possibilities:
The second approach is definitely simpler ;-) however it would change dynamics of your task.
- manually compute gripper's center of mass, total mass \(m\), then express gripper's center of mass \(\vec{r}\) and gravity vector \(\vec{g}\) in force sensor's reference frame, and compute resulting force/torque (remember that torque \(\vec\tau=\vec{r} \times \left(m \vec{g}\right)\))
- add an anti-gravity force to the gripper (sim.addForce)
Thank you for your answer.
You have used two ways to compensate for gravity and I understand how to calculate it, thank you very much for your detailed reply.
Best regards,
Robot_XiaoXi
-
- Posts: 6
- Joined: 20 Mar 2023, 14:50
Re: Gravity compensation problem of force sensor in CoppeliaSim
hello fferri,fferri wrote: ↑22 Mar 2023, 16:16 For the record, to implement first proposed approach, you would first compute a list of dynamic shapes that you want to exclude from force sensor's reading, for which you'll be computing force and torque manually:
then compute mass and center of mass of those shapes (in absolute coords):Code: Select all
allNonStaticShapes={} -- add items to 'allNonStaticShapes'
then convert into local coords (sensor's):Code: Select all
com,mass=Vector{0,0,0},0 for i,shape in ipairs(allNonStaticShapes) do local mass1=sim.getShapeMass(shape) local inertia,comFrame=sim.getShapeInertia(shape) local shapeFrame=sim.getObjectMatrix(shape,sim.handle_world) local m=sim.multiplyMatrices(shapeFrame,comFrame) com=com+mass1*Vector{m[4],m[8],m[12]} mass=mass+mass1 end if mass~=0 then com=com/mass end
and finally compute force and torque caused by those shapes:Code: Select all
minv=sim.getObjectMatrix(sensorHandle,sim.handle_world);sim.invertMatrix(minv) minv=Matrix4x4(minv) com=minv:mult(com) minv[1][4]=0 minv[2][4]=0 minv[3][4]=0 g=minv:mult(Vector{0,0,-9.81})
Code: Select all
force=g*mass torque=com:cross(force)
force
andtorque
are also in sensor's reference frame.
Please note that everything's said is only valid for a static configuration. If the robot moves, there are other accelerations that are not taken into account.
Thank you for your reply.
For the static power compensation of the above system, I have successfully done it according to the method you provided. If the robot moves, how should I compensate dynamically?
Best regards,
Robot_XiaoXi
Re: Gravity compensation problem of force sensor in CoppeliaSim
In dynamic conditions you would have to consider:
-
- Posts: 6
- Joined: 20 Mar 2023, 14:50