pavel/teleoperation
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

small update

Browse Source
This commit is contained in:
HRS_D
2019-02-05 13:44:24 +01:00
parent 193dcff7a8
commit 3b16ae6fcd
2 changed files with 26 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
script/cartesian_controller.py
View File

@@ -73,12 +73,29 @@ def pseudo_inverse(jacobian):
def reference_generator(p_d)
# calculate jacobian
jac_mat = jacobian()
# use jacobian to compute desired joint speed
derivative_speed = (p_d - end_position) / 5
inv_jac = pseudo_inverse(jac_mat)
angular_velocity = derivative_speed * inv_jac
# integrate over desired speed to get desired joint position
names = "LArm"
useSensors = False
commandAngles = motionProxy.getAngles(names, useSensors)
names = "RArm"
useSensors = False
commandAngles = motionProxy.getAngles(names, useSensors)
goal_angle = mom_angle + (angular_velocity * 5)
# return desired joint position and speed
return