Scribd
Upload
16 views3 pages

Hybrid Force-Position Controller

Uploaded by

nupurranjan4
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
16 views3 pages

Hybrid Force-Position Controller

Uploaded by

nupurranjan4
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 3

Hybrid Force-Position Controller

Psuedo Code for 'hybrid_force_position_control.py'

Intialise:
PD parameters for Force Control: Kp_F, Kd_F
PD parameters for Position Control: Kp_P, Kd_P
Task Matrices: pose_task_matrix, force_task_matrix

while True:
Update Reference Wrench and Pose vectors: F_r & P_r
Update Instantenous Wrench and Pose vectors: F_i & P_i

Calculate: 'dt' of this loop, 'd(error_F)', 'd(error_P)' change in error


in one loop
Calculate: 'R' Rotation Matrix from eef to base

error_F = F_i - F_r


vel_F_p = Kp_F * error_F #
Proportional
vel_F_d = Kd_F * d(error_F)/dt #
Derivative
vel_F = vel_F_p + vel_F_d

q_r = P_r[Top 4 Value]


q_i = P_i[Top 4 Value]
q_diff = SubstractQuaternion(q_r, q_i)

error_P[Top 3 Value] = ConvertToAxisAngleFromQuaternion(q_diff)


error_P[Botton 3 Value] = P_r[Top 3 Value] - P_i[Top 3 Value]

vel_P_p_eef = Kp_P * error_P #


Proportional
vel_P_d_eef = Kd_P * d(error_P)/dt #
Derivative

vel_P_eef = vel_P_p_eef + vel_P_d_eef


vel_P = R * vel_P_eef

vel = pose_task_matrix * vel_P + force_task_matrix * vel_F


Send 'vel' vector to 'velocity_control.py' script

Working of SubstractQuaternion(q_r, q_i)

if

q_r ⋅ q_i < 0

then

q_r = −q_r

Finally,

q_diff = q_r ∗ q_i−1

Psuedo Code for 'velocity_control.py'

while True:
Get 'vel' vector from 'hybrid_force_position_control.py' script
Calculate current Psuedo Inverse of Jacobian Matrix: J_inv
Calculate: 'dt' of the loop
Joint Velocity: w = J_inv * vel
Joint Angles q = q + w * dt
Publish Joint Angles 'q' to Effort Controller

You might also like

pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy