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 update(self):
# motor_commands = self.get_god_map().get_data(identifier.cmd)
# if motor_commands:
# for joint_position_identifier in motor_commands:
# joint_velocity_identifier = list(joint_position_identifier[:-1]) + [u'velocity']
# current_position = self.get_god_map().get_data(joint_position_identifier)
# self.get_god_map().set_data(joint_position_identifier, current_position)
# self.get_god_map().set_data(joint_velocity_identifier, 0)
# pass
motor_commands = self.get_god_map().get_data(identifier.cmd)
current_js = self.get_god_map().get_data(identifier.joint_states)
next_js = None
if motor_commands:
next_js = OrderedDict()
for joint_name, sjs in current_js.items():
# if joint_name in motor_commands:
# cmd = motor_commands[joint_name]
# else:
cmd = 0.0
next_js[joint_name] = SingleJointState(sjs.name,
sjs.position + cmd,
velocity=cmd /
self.sample_period)
if next_js is not None:
self.get_god_map().set_data(identifier.joint_states, next_js)
else:
self.get_god_map().set_data(identifier.joint_states, current_js)
self.get_god_map().set_data(identifier.last_joint_states, current_js)
return Status.SUCCESS
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 test_boxy_fk1(self, parsed_boxy, js):
kdl = KDL(boxy_urdf())
root = u'base_footprint'
tips = [u'left_gripper_tool_frame', u'right_gripper_tool_frame']
for tip in tips:
kdl_r = kdl.get_robot(root, tip)
kdl_fk = kdl_to_pose(kdl_r.fk(js))
mjs = {}
for joint_name, position in js.items():
mjs[joint_name] = SingleJointState(joint_name, position)
parsed_boxy.joint_state = mjs
symengine_fk = parsed_boxy.get_fk_pose(root, tip).pose
compare_poses(kdl_fk, symengine_fk)
def test_pr2_fk1(self, parsed_pr2, js):
"""
:type parsed_pr2: Robot
:type js:
:return:
"""
kdl = KDL(pr2_urdf())
root = u'base_link'
tips = [u'l_gripper_tool_frame', u'r_gripper_tool_frame']
for tip in tips:
kdl_r = kdl.get_robot(root, tip)
kdl_fk = kdl_to_pose(kdl_r.fk(js))
mjs = {}
for joint_name, position in js.items():
mjs[joint_name] = SingleJointState(joint_name, position)
parsed_pr2.joint_state = mjs
symengine_fk = parsed_pr2.get_fk_pose(root, tip).pose
compare_poses(kdl_fk, symengine_fk)
def update(self):
motor_commands = self.get_god_map().get_data(identifier.cmd)
current_js = self.get_god_map().get_data(identifier.joint_states)
next_js = None
if motor_commands:
next_js = OrderedDict()
for joint_name, sjs in current_js.items():
if joint_name in motor_commands:
cmd = motor_commands[joint_name]
else:
cmd = 0.0
next_js[joint_name] = SingleJointState(sjs.name, sjs.position + cmd,
velocity=cmd/self.sample_period)
if next_js is not None:
self.get_god_map().set_data(identifier.joint_states, next_js)
else:
self.get_god_map().set_data(identifier.joint_states, current_js)
self.get_god_map().set_data(identifier.last_joint_states, current_js)
return Status.RUNNING
def update(self):
self.time += self.frequency
motor_commands = self.god_map.get_data([self.next_cmd_identifier])
current_js = self.god_map.get_data([self.js_identifier])
if motor_commands is not None:
self.next_js = OrderedDict()
for joint_name, sjs in current_js.items():
if joint_name in motor_commands:
cmd = motor_commands[joint_name]
else:
cmd = 0.0
self.next_js[joint_name] = SingleJointState(
sjs.name,
sjs.position + cmd * self.frequency,
velocity=cmd)
if self.next_js is not None:
self.god_map.set_data([self.js_identifier], self.next_js)
else:
self.god_map.set_data([self.js_identifier], current_js)
self.god_map.set_data([self.time_identifier], self.time)
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
