def get_joint_states(self):
mjs = dict()
for joint_info in self.joint_name_to_info.values():
if joint_info.joint_type in [p.JOINT_REVOLUTE, p.JOINT_PRISMATIC]:
sjs = SingleJointState()
sjs.name = joint_info.joint_name
sjs.position = p.getJointState(self.id,
joint_info.joint_index)[0]
mjs[sjs.name] = sjs
return mjs
def generate_joint_state(self, f):
"""
:param f: lambda joint_info: float
:return:
"""
js = {}
for joint_name in sorted(self.get_movable_joints()):
sjs = SingleJointState()
sjs.name = joint_name
sjs.position = f(joint_name)
js[joint_name] = sjs
return js
def generate_joint_state(self, f):
"""
:param f: lambda joint_info: float
:return:
"""
# TODO possible optimization, if some joints are not controlled, the collision matrix might get smaller
js = {}
for joint_name in self.get_controllable_joints():
sjs = SingleJointState()
sjs.name = joint_name
sjs.position = f(joint_name)
js[joint_name] = sjs
return js
def generate_joint_state(self, f):
"""
:param f: lambda joint_info: float
:return:
"""
js = {}
for joint_name, joint_info in self.joint_name_to_info.items():
if joint_info.joint_type in [
JOINT_REVOLUTE, JOINT_PRISMATIC, JOINT_PLANAR,
JOINT_SPHERICAL
]:
sjs = SingleJointState()
sjs.name = joint_name
sjs.position = f(joint_info)
js[joint_name] = sjs
return js
def to_joint_state_dict(msg):
"""
Converts a ROS message of type sensor_msgs/JointState into an instance of MultiJointState.
:param msg: ROS message to convert.
:type msg: JointState
:return: Corresponding MultiJointState instance.
:rtype: OrderedDict[str, SingleJointState]
"""
mjs = OrderedDict()
for i, joint_name in enumerate(msg.name):
sjs = SingleJointState()
sjs.name = joint_name
sjs.position = msg.position[i]
try:
sjs.velocity = msg.velocity[i]
except IndexError:
sjs.velocity = 0
try:
sjs.effort = msg.effort[i]
except IndexError:
sjs.effort = 0
mjs[joint_name] = sjs
return mjs