def __init__(self, robot_file, eef_name, x, y, z):
self.robot = Robot()
self.robot.load_from_urdf_path(robot_file, 'base_link', [eef_name])
self.controller = MyPositionController(self.robot, eef_name)
self.controller.set_goal(x,y,z)
self.cmd_pub = rospy.Publisher('simulator/commands', JointState, queue_size=1)
self.cmd_sub = rospy.Subscriber('/joint_states', JointState, self.js_callback, queue_size=1)
class MyPositionControllerNode(object):
def __init__(self, robot_file, eef_name, x, y, z):
self.robot = Robot()
self.robot.load_from_urdf_path(robot_file, 'base_link', [eef_name])
self.controller = MyPositionController(self.robot, eef_name)
self.controller.set_goal(x,y,z)
self.cmd_pub = rospy.Publisher('simulator/commands', JointState, queue_size=1)
self.cmd_sub = rospy.Subscriber('/joint_states', JointState, self.js_callback, queue_size=1)
def js_callback(self, joint_state):
js_dict = {joint_state.name[x]: joint_state.position[x] for x in range(len(joint_state.name))}
self.robot.set_joint_state(js_dict)
cmd = self.controller.get_next_command()
cmd_msg = JointState()
cmd_msg.header.stamp = rospy.Time.now()
for joint_name, velocity in cmd.items():
cmd_msg.name.append(joint_name)
cmd_msg.velocity.append(velocity)
cmd_msg.position.append(0)
cmd_msg.effort.append(0)
self.cmd_pub.publish(cmd_msg)
def add_robot(self, robot, base_pose, controlled_joints, ignored_pairs, added_pairs):
"""
:type robot: giskardpy.world_object.WorldObject
:type controlled_joints: list
:type base_pose: PoseStamped
"""
if not isinstance(robot, WorldObject):
raise TypeError(u'only WorldObject can be added to world')
if self.has_robot():
raise RobotExistsException(u'A robot is already loaded')
if self.has_object(robot.get_name()):
raise DuplicateNameException(
u'can\'t add robot; object with name "{}" already exists'.format(robot.get_name()))
self._robot = Robot.from_urdf_object(urdf_object=robot,
base_pose=base_pose,
controlled_joints=controlled_joints,
path_to_data_folder=self._path_to_data_folder,
ignored_pairs=ignored_pairs,
added_pairs=added_pairs)
logging.loginfo(u'--> added {} to world'.format(robot.get_name()))
def pr2_joint_state(draw):
pr2 = Robot.from_urdf_file(pr2_urdf())
return draw(rnd_joint_state(*pr2.get_joint_limits()))
def parsed_boxy(function_setup):
"""
:rtype: Robot
"""
return Robot(boxy_urdf())
def parsed_donbot(function_setup):
"""
:rtype: Robot
"""
return Robot(donbot_urdf())
def parsed_base_bot(function_setup):
"""
:rtype: Robot
"""
return Robot(base_bot_urdf())
def parsed_pr2(function_setup):
"""
:rtype: Robot
"""
return Robot(pr2_urdf())
class TestSymengineController(object):
pr2_joint_limits = Robot(pr2_urdf()).get_all_joint_limits()
donbot_joint_limits = Robot(donbot_urdf()).get_all_joint_limits()
boxy_joint_limits = Robot(boxy_urdf()).get_all_joint_limits()
def test_constraints_pr2(self, parsed_pr2):
assert len(parsed_pr2.hard_constraints) == 28
assert len(parsed_pr2.joint_constraints) == 48
def test_constraints_donbot(self, parsed_donbot):
assert len(parsed_donbot.hard_constraints) == 9
assert len(parsed_donbot.joint_constraints) == 11
def test_constraints_boxy(self, parsed_boxy):
assert len(parsed_boxy.hard_constraints) == 26
assert len(parsed_boxy.joint_constraints) == 26
@given(rnd_joint_state(pr2_joint_limits))
def test_pr2_fk1(self, parsed_pr2, js):
"""
:type parsed_pr2: Robot
:type js:
:return:
"""
kdl = KDL(pr2_urdf())
root = u'odom_combined'
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)
@given(rnd_joint_state(donbot_joint_limits))
def test_donbot_fk1(self, parsed_donbot, js):
kdl = KDL(donbot_urdf())
root = u'base_footprint'
tips = [u'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_donbot.joint_state = mjs
symengine_fk = parsed_donbot.get_fk_pose(root, tip).pose
compare_poses(kdl_fk, symengine_fk)
@given(rnd_joint_state(boxy_joint_limits))
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_get_controllable_joint_names_pr2(self, parsed_pr2):
expected = {
u'l_shoulder_pan_joint', u'br_caster_l_wheel_joint',
u'r_gripper_l_finger_tip_joint', u'r_elbow_flex_joint',
u'torso_lift_joint', u'r_gripper_l_finger_joint',
u'r_forearm_roll_joint', u'l_gripper_r_finger_tip_joint',
u'r_shoulder_lift_joint', u'fl_caster_rotation_joint',
u'l_gripper_motor_screw_joint', u'r_wrist_roll_joint',
u'r_gripper_motor_slider_joint', u'l_forearm_roll_joint',
u'r_gripper_joint', u'bl_caster_rotation_joint',
u'fl_caster_r_wheel_joint', u'l_shoulder_lift_joint',
u'head_pan_joint', u'head_tilt_joint', u'fr_caster_l_wheel_joint',
u'fl_caster_l_wheel_joint', u'l_gripper_motor_slider_joint',
u'br_caster_r_wheel_joint', u'r_gripper_motor_screw_joint',
u'r_upper_arm_roll_joint', u'fr_caster_rotation_joint',
u'torso_lift_motor_screw_joint', u'bl_caster_l_wheel_joint',
u'r_wrist_flex_joint', u'r_gripper_r_finger_tip_joint',
u'l_elbow_flex_joint', u'laser_tilt_mount_joint',
u'r_shoulder_pan_joint', u'fr_caster_r_wheel_joint',
u'l_wrist_roll_joint', u'r_gripper_r_finger_joint',
u'bl_caster_r_wheel_joint', u'l_gripper_joint',
u'l_gripper_l_finger_tip_joint', u'br_caster_rotation_joint',
u'l_gripper_l_finger_joint', u'l_wrist_flex_joint',
u'l_upper_arm_roll_joint', u'l_gripper_r_finger_joint',
u'odom_x_joint', u'odom_y_joint', u'odom_z_joint'
}
assert set(parsed_pr2.get_joint_names_controllable()).difference(
expected) == set()
def test_get_joint_names_donbot(self, parsed_donbot):
expected = {
u'ur5_wrist_3_joint', u'ur5_elbow_joint', u'ur5_wrist_1_joint',
u'odom_z_joint', u'ur5_shoulder_lift_joint', u'odom_y_joint',
u'ur5_wrist_2_joint', u'odom_x_joint', u'ur5_shoulder_pan_joint',
u'gripper_joint', u'refills_finger_joint'
}
assert set(parsed_donbot.get_joint_names_controllable()).difference(
expected) == set()
def test_get_joint_names_boxy(self, parsed_boxy):
expected = {
u'left_arm_2_joint', u'neck_joint_end', u'neck_wrist_1_joint',
u'right_arm_2_joint', u'right_arm_4_joint', u'neck_wrist_3_joint',
u'right_arm_3_joint', u'right_gripper_base_gripper_right_joint',
u'left_gripper_base_gripper_right_joint', u'left_arm_0_joint',
u'right_gripper_base_gripper_left_joint', u'left_arm_4_joint',
u'left_arm_6_joint', u'right_arm_1_joint', u'left_arm_1_joint',
u'neck_wrist_2_joint', u'triangle_base_joint', u'neck_elbow_joint',
u'right_arm_5_joint', u'left_arm_3_joint',
u'neck_shoulder_pan_joint', u'right_arm_0_joint',
u'neck_shoulder_lift_joint', u'left_arm_5_joint',
u'left_gripper_base_gripper_left_joint', u'right_arm_6_joint'
}
assert set(parsed_boxy.get_joint_names_controllable()).difference(
expected) == set()