Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Python client set_pos setting up all joints #32

Closed
prajankya opened this issue Sep 13, 2019 · 2 comments
Closed

Python client set_pos setting up all joints #32

prajankya opened this issue Sep 13, 2019 · 2 comments
Assignees
@adnanmunawar @nag92

Comments

@prajankya
Copy link
Contributor

I was using python client to set position for Kuka robot (./ambf_simulator -l 8 i.e. blender-kuka.yaml)

#! /usr/bin/env python

import time

from ambf_client import Client

_client = Client()

_client.connect()

print('\n\n----')
print("List of Objects")
print(_client.get_obj_names())

kuka_handle = _client.get_obj_handle('base')

time.sleep(0.2)
print('\n\n----')

print('Kuka Base Pos:')
print(kuka_handle.get_pos())

print('\n\n----')

print('Number of Joints in kuka:')
print(kuka_handle.get_num_joints())

print('Name of kuka\'s children:')
print(kuka_handle.get_joint_names())


print(kuka_handle.get_name())
print(kuka_handle.get_pos_command())
print(kuka_handle.get_pose())

kuka_handle.set_joint_pos(0, 1.57)
# kuka_handle.set_joint_effort(0, 10)
# kuka_handle.set_joint_effort(1, 10)

print('\n\n----')
raw_input("Let's clean up. Press Enter to continue...")
# Lastly to cleanup
_client.clean_up()

Also the blender-kuka.yaml file was updated to change PID values for all joints as a work around for the issue.
blender-kuka.yaml

# AMBF Version: (0.1)
# Generated By: ambf_addon for Blender (2.79.0)
# Link: https://github.com/WPI-AIM/ambf_addon
# Generated on: 2019-04-01 17:46:58
bodies: [BODY base, BODY link1, BODY link2, BODY link3, BODY link4, BODY link5, BODY
    link6, BODY link7]
joints: [JOINT base-link1, JOINT link1-link2, JOINT link2-link3, JOINT link3-link4,
  JOINT link4-link5, JOINT link5-link6, JOINT link6-link7]
high resolution path: ../../../meshes/blender_afmb/kuka_lwr/high_res/
low resolution path: ../../../meshes/blender_afmb/kuka_lwr/low_res/
ignore inter-collision: true
namespace: /ambf/env/
joint erp: 0.5
joint cfm: 0.3
BODY base:
  name: base
  mesh: base.STL
  mass: 0.0
  collision margin: 0.001
  scale: 1.0
  location:
    orientation: {p: -0.0, r: 0.0, y: 0.0}
    position: {x: 0.0, y: 0.0, z: -1.3}
  inertial offset:
    orientation: {p: 0, r: 0, y: 0}
    position: {x: 0.001, y: -0.0, z: 0.06}
  publish joint names: true
  publish joint positions: true
  friction: {rolling: 0.01, static: 0.5}
  damping: {angular: 0.95, linear: 0.95}
  restitution: 0
  collision groups: [0]
  color components:
    ambient: {level: 1.0}
    diffuse: {b: 0.0147, g: 0.0147, r: 0.0147}
    specular: {b: 1.0, g: 1.0, r: 1.0}
    transparency: 1.0
BODY link1:
  name: link1
  mesh: link1.STL
  mass: 1.0
  collision margin: 0.001
  scale: 1.0
  location:
    orientation: {p: -0.0, r: 0.0, y: 0.0}
    position: {x: 0.0, y: 0.0, z: -1.197}
  inertial offset:
    orientation: {p: 0, r: 0, y: 0}
    position: {x: 0.0, y: -0.017, z: 0.134}
  friction: {rolling: 0.01, static: 0.5}
  damping: {angular: 0.95, linear: 0.95}
  restitution: 0
  collision groups: [0]
  color components:
    ambient: {level: 1.0}
    diffuse: {b: 0.0054, g: 0.2702, r: 0.8}
    specular: {b: 1.0, g: 1.0, r: 1.0}
    transparency: 1.0
BODY link2:
  name: link2
  mesh: link2.STL
  mass: 1.0
  collision margin: 0.001
  scale: 1.0
  location:
    orientation: {p: -0.0, r: -1.571, y: 0.0}
    position: {x: 0.0, y: 0.013, z: -0.988}
  inertial offset:
    orientation: {p: 0, r: 0, y: 0}
    position: {x: 0.0, y: -0.074, z: 0.009}
  friction: {rolling: 0.01, static: 0.5}
  damping: {angular: 0.95, linear: 0.95}
  restitution: 0
  collision groups: [0]
  color components:
    ambient: {level: 1.0}
    diffuse: {b: 0.0647, g: 0.5619, r: 0.8}
    specular: {b: 1.0, g: 1.0, r: 1.0}
    transparency: 1.0
BODY link3:
  name: link3
  mesh: link3.STL
  mass: 1.0
  collision margin: 0.001
  scale: 1.0
  location:
    orientation: {p: -0.0, r: -0.0, y: 0.0}
    position: {x: 0.0, y: 0.004, z: -0.794}
  inertial offset:
    orientation: {p: 0, r: 0, y: 0}
    position: {x: 0.0, y: 0.017, z: 0.134}
  friction: {rolling: 0.01, static: 0.5}
  damping: {angular: 0.95, linear: 0.95}
  restitution: 0
  collision groups: [0]
  color components:
    ambient: {level: 1.0}
    diffuse: {b: 0.005, g: 0.3311, r: 1.0}
    specular: {b: 1.0, g: 1.0, r: 1.0}
    transparency: 1.0
BODY link4:
  name: link4
  mesh: link4.STL
  mass: 1.0
  collision margin: 0.001
  scale: 1.0
  location:
    orientation: {p: -0.0, r: 1.571, y: 0.0}
    position: {x: 0.0, y: -0.009, z: -0.592}
  inertial offset:
    orientation: {p: 0, r: 0, y: 0}
    position: {x: -0.001, y: 0.081, z: 0.008}
  friction: {rolling: 0.01, static: 0.5}
  damping: {angular: 0.95, linear: 0.95}
  restitution: 0
  collision groups: [0]
  color components:
    ambient: {level: 1.0}
    diffuse: {b: 0.0, g: 0.4756, r: 0.8}
    specular: {b: 1.0, g: 1.0, r: 1.0}
    transparency: 1.0
BODY link5:
  name: link5
  mesh: link5.STL
  mass: 1.0
  collision margin: 0.001
  scale: 1.0
  location:
    orientation: {p: -0.0, r: -0.0, y: 0.0}
    position: {x: -0.002, y: -0.001, z: -0.39}
  inertial offset:
    orientation: {p: 0, r: 0, y: 0}
    position: {x: 0.0, y: -0.017, z: 0.129}
  friction: {rolling: 0.01, static: 0.5}
  damping: {angular: 0.95, linear: 0.95}
  restitution: 0
  collision groups: [0]
  color components:
    ambient: {level: 1.0}
    diffuse: {b: 0.005, g: 0.3119, r: 0.9406}
    specular: {b: 1.0, g: 1.0, r: 1.0}
    transparency: 1.0
BODY link6:
  name: link6
  mesh: link6.STL
  mass: 1.0
  collision margin: 0.001
  scale: 1.0
  location:
    orientation: {p: -0.0, r: -1.571, y: 0.0}
    position: {x: 0.0, y: -0.053, z: -0.186}
  inertial offset:
    orientation: {p: 0, r: 0, y: 0}
    position: {x: -0.0, y: 0.007, z: 0.068}
  friction: {rolling: 0.01, static: 0.5}
  damping: {angular: 0.95, linear: 0.95}
  restitution: 0
  collision groups: [0]
  color components:
    ambient: {level: 1.0}
    diffuse: {b: 0.0167, g: 0.5171, r: 0.8}
    specular: {b: 1.0, g: 1.0, r: 1.0}
    transparency: 1.0
BODY link7:
  name: link7
  mesh: link7.STL
  mass: 1.0
  collision margin: 0.001
  scale: 1.0
  location:
    orientation: {p: -0.0, r: -0.0, y: 0.0}
    position: {x: -0.003, y: 0.0, z: -0.136}
  inertial offset:
    orientation: {p: 0, r: 0, y: 0}
    position: {x: 0.006, y: 0.0, z: 0.015}
  friction: {rolling: 0.01, static: 0.5}
  damping: {angular: 0.95, linear: 0.95}
  restitution: 0
  collision groups: [0]
  color components:
    ambient: {level: 1.0}
    diffuse: {b: 0.01, g: 0.01, r: 0.01}
    specular: {b: 1.0, g: 1.0, r: 1.0}
    transparency: 1.0
JOINT base-link1:
  name: base-link1
  parent: BODY base
  child: BODY link1
  parent axis: {x: 0.0, y: 0.0, z: 1.0}
  parent pivot: {x: 0.0, y: 0.0, z: 0.103}
  child axis: {x: 0.0, y: 0.0, z: 1.0}
  child pivot: {x: 0.0, y: 0.0, z: 0.0}
  joint limits: {high: 2.094, low: -2.094}
  controller: {D: 0.1, I: 0, P: 100.0}
  type: revolute
JOINT link1-link2:
  name: link1-link2
  parent: BODY link1
  child: BODY link2
  parent axis: {x: 0.0, y: 1.0, z: -0.0}
  parent pivot: {x: 0.0, y: 0.013, z: 0.209}
  child axis: {x: 0.0, y: 0.0, z: 1.0}
  child pivot: {x: 0.0, y: 0.0, z: 0.0}
  joint limits: {high: 2.094, low: -2.094}
  controller: {D: 0.1, I: 0, P: 100.0}
  type: revolute
JOINT link2-link3:
  name: link2-link3
  parent: BODY link2
  child: BODY link3
  parent axis: {x: 0.0, y: -1.0, z: 0.0}
  parent pivot: {x: 0.0, y: -0.194, z: -0.009}
  child axis: {x: 0.0, y: 0.0, z: 1.0}
  child pivot: {x: 0.0, y: 0.0, z: 0.0}
  joint limits: {high: 2.094, low: -2.094}
  controller: {D: 0.1, I: 0, P: 100.0}
  type: revolute
JOINT link3-link4:
  name: link3-link4
  parent: BODY link3
  child: BODY link4
  parent axis: {x: 0.0, y: -1.0, z: 0.0}
  parent pivot: {x: 0.0, y: -0.013, z: 0.202}
  child axis: {x: 0.0, y: 0.0, z: 1.0}
  child pivot: {x: 0.0, y: 0.0, z: 0.0}
  joint limits: {high: 2.094, low: -2.094}
  controller: {D: 0.1, I: 0, P: 100.0}
  type: revolute
JOINT link4-link5:
  name: link4-link5
  parent: BODY link4
  child: BODY link5
  parent axis: {x: 0.0, y: 1.0, z: 0.0}
  parent pivot: {x: -0.002, y: 0.202, z: -0.008}
  child axis: {x: 0.0, y: 0.0, z: 1.0}
  child pivot: {x: 0.0, y: 0.0, z: 0.0}
  joint limits: {high: 2.094, low: -2.094}
  controller: {D: 0.1, I: 0, P: 100.0}
  type: revolute
JOINT link5-link6:
  name: link5-link6
  parent: BODY link5
  child: BODY link6
  parent axis: {x: 0.0, y: 1.0, z: 0.0}
  parent pivot: {x: 0.002, y: -0.052, z: 0.204}
  child axis: {x: 0.0, y: 0.0, z: 1.0}
  child pivot: {x: 0.0, y: 0.0, z: 0.0}
  joint limits: {high: 2.094, low: -2.094}
  controller: {D: 0.1, I: 0, P: 100.0}
  type: revolute
JOINT link6-link7:
  name: link6-link7
  parent: BODY link6
  child: BODY link7
  parent axis: {x: 0.0, y: -1.0, z: 0.0}
  parent pivot: {x: -0.003, y: -0.05, z: 0.053}
  child axis: {x: 0.0, y: 0.0, z: 1.0}
  child pivot: {x: 0.0, y: 0.0, z: 0.0}
  joint limits: {high: 3.054, low: -3.054}
  controller: {D: 0.1, I: 0, P: 100.0}
  type: revolute

After running the python, below is the terminal output

----
List of Objects
['link5', 'link4', 'link7', 'link6', 'link1', 'link3', 'link2', 'lights/light_left', 'base', 'default_camera', 'World', 'lights/light_right']


----
Kuka Base Pos:
x: -0.001
y: 0.0
z: -1.36


----
Number of Joints in kuka:
7
 
Name of kuka's children:
['base-link1', 'link1-link2', 'link2-link3', 'link3-link4', 'link4-link5', 'link5-link6', 'link6-link7']
base
x: -0.001
y: 0.0
z: -1.36
[-0.001, 0.0, -1.36, -0.0, 0.0, -0.0]


----
Let's clean up. Press Enter to continue...
Closing publisher for:  link5
Closing publisher for:  link4
Closing publisher for:  link7
Closing publisher for:  link6
Closing publisher for:  link1
Closing publisher for:  link3
Closing publisher for:  link2
Closing publisher for:  lights/light_left
Closing publisher for:  base
Closing publisher for:  default_camera
Closing publisher for:  World
Closing publisher for:  lights/light_right

Below is the output of running rostopic echo /ambf/env/base/Command

---
header: 
  seq: 201578
  stamp: 
    secs: 1568321596
    nsecs: 638394117
  frame_id: ''
enable_position_controller: False
pose: 
  position: 
    x: 0.0
    y: 0.0
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 0.0
wrench: 
  force: 
    x: 0.0
    y: 0.0
    z: 0.0
  torque: 
    x: 0.0
    y: 0.0
    z: 0.0
joint_cmds: [1.5700000524520874, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position_controller_mask: [True, True, True, True, True, True, True]
publish_children_names: False
publish_joint_names: False
publish_joint_positions: False

Ambf simulator is being sent all the joint positions from ambf_client, which as discussed with @adnanmunawar is not how it is should be.
@adnanmunawar
Copy link
Member

Thanks for creating the issue. While trying to fix the overriding of joint cmds in 5174fae we accidentally introduced this issue

@adnanmunawar
Copy link
Member

This line:

ambf/ambf_ros_modules/ambf_comm/scripts/ambf_object.py

Line 333 in 03e2fa6

self._cmd.position_controller_mask = [1]*n_jnts

Needs to be changed from

            self._cmd.position_controller_mask = [1]*n_jnts

To

            self._cmd.position_controller_mask = [0]*n_jnts

@nag92

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees

@adnanmunawar adnanmunawar

@nag92 nag92

Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
3 participants
@prajankya @adnanmunawar @nag92