def __init__(self, urdf):
if urdf.endswith(u'.urdfs'):
with open(urdf, u'r') as file:
urdf = file.read()
r = URDF.from_xml_string(hacky_urdf_parser_fix(urdf))
self.tree = kdl_tree_from_urdf_model(r)
self.robots = {}
def __init__(
self,
urdf_relative_path='../../assets/urdf/sawyer_arm_intera.urdf'):
'''
Uses inverse kinematics in order to go from end-effector cartesian velocity commands to joint velocity commands
:param urdf_relative_path: The path to the URDF file of the robot, relative to the current directory
'''
self.num_joints = 7
# Get the URDF
urdf_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),
urdf_relative_path)
self.urdf_model = URDF.from_xml_string(open(urdf_path).read())
# Set the base link, and the tip link. I think that these are just strings, defined somewhere in the URDF. The name 'right_hand' is how the gripper is described on the real Sawyer, but it may have a different name in your own URDF.
self.base_link = self.urdf_model.get_root()
self.tip_link = "right_hand"
# # Create a KDL tree from the URDF model. This is an object defining the kinematics of the entire robot.
self.kdl_tree = treeFromUrdfModel(self.urdf_model)
# Create a KDL chain from the tree. This is one specific chain within the overall kinematics model.
self.arm_chain = self.kdl_tree[1].getChain(self.base_link,
self.tip_link)
# Create a solver which will be used to compute the forward kinematics
self.forward_kinematics_solver = PyKDL.ChainFkSolverPos_recursive(
self.arm_chain)
# Create a solver which will be used to compute the Jacobian
self.jacobian_solver = PyKDL.ChainJntToJacSolver(self.arm_chain)
#Velocity inverse kinematics solver
self.IK_v_solver = PyKDL.ChainIkSolverVel_pinv(self.arm_chain)
# Create a solver to retrieve the joint angles form the eef position
self.IK_solver = PyKDL.ChainIkSolverPos_NR(
self.arm_chain, self.forward_kinematics_solver, self.IK_v_solver)
def from_vrx_urdf(cls, urdf_string):
urdf = URDF.from_xml_string(urdf_string)
buff = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(buff) # noqa
names = []
positions = []
angles = []
for link in urdf.links:
find = link.name.find('_propeller_link')
if find == -1:
continue
name = link.name[:find]
try:
trans = buff.lookup_transform('base_link', link.name,
rospy.Time(), rospy.Duration(10))
except tf2_ros.TransformException as e:
raise Exception(e)
translation = rosmsg_to_numpy(trans.transform.translation)
rot = rosmsg_to_numpy(trans.transform.rotation)
yaw = euler_from_quaternion(rot)[2]
names.append(name)
positions.append(translation[0:2])
angles.append(yaw)
return cls(names, positions, angles, vrx_force_to_command,
(250., -100.))
def __init__(self):
self.num_joints = 7
# Get the URDF (you will need to write your own "load_urdf()" function)
urdf_path = os.path.join(assets_root,
'./kdl/panda_urdf/panda_arm_hand.urdf')
# urdf_path= os.path.join(robosuite.models.assets_root, "bullet_data/sawyer_description/urdf/sawyer_intera.urdf")
self.urdf_model = URDF.from_xml_string(
open(urdf_path).read().encode(
'ascii')) # encode to ascii solve the error
#self.kdl_tree = treeFromFile(urdf_path)
# Set the base link, and the tip link. I think that these are just strings, defined somewhere in the URDF. The name 'right_hand' is how the gripper is described on the real Sawyer, but it may have a different name in your own URDF.
self.base_link = self.urdf_model.get_root()
self.tip_link = "panda_hand" # hand of panda robot, see panda_arm_hand.urdf
# # Create a KDL tree from the URDF model. This is an object defining the kinematics of the entire robot.
self.kdl_tree = treeFromUrdfModel(self.urdf_model)
# Create a KDL chain from the tree. This is one specific chain within the overall kinematics model.
self.arm_chain = self.kdl_tree[1].getChain(self.base_link,
self.tip_link)
# Create a solver which will be used to compute the forward kinematics
self.forward_kinematics_solver = PyKDL.ChainFkSolverPos_recursive(
self.arm_chain)
# Create a solver which will be used to compute the Jacobian
self.jacobian_solver = PyKDL.ChainJntToJacSolver(self.arm_chain)
#Velocity inverse kinematics solver
self.IK_v_solver = PyKDL.ChainIkSolverVel_pinv(self.arm_chain)
# Create a solver to retrieve the joint angles form the eef position
self.IK_solver = PyKDL.ChainIkSolverPos_NR(
self.arm_chain, self.forward_kinematics_solver, self.IK_v_solver)
def __init__(self):
self.num_joints = 7
# Get the URDF
urdf_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'../assets/urdf/sawyer_intera.urdf')
self.urdf_model = URDF.from_xml_string(open(urdf_path).read())
# Set the base link, and the tip link.
self.base_link = self.urdf_model.get_root()
self.tip_link = "right_hand"
# # Create a KDL tree from the URDF model. This is an object defining the kinematics of the entire robot.
self.kdl_tree = treeFromUrdfModel(self.urdf_model)
# Create a KDL chain from the tree. This is one specific chain within the overall kinematics model.
self.arm_chain = self.kdl_tree[1].getChain(self.base_link,
self.tip_link)
# Create a solver which will be used to compute the forward kinematics
self.forward_kinematics_solver = PyKDL.ChainFkSolverPos_recursive(
self.arm_chain)
# Create a solver which will be used to compute the Jacobian
self.jacobian_solver = PyKDL.ChainJntToJacSolver(self.arm_chain)
#Velocity inverse kinematics solver
self.IK_v_solver = PyKDL.ChainIkSolverVel_pinv(self.arm_chain)
# Create a solver to retrieve the joint angles form the eef position
self.IK_solver = PyKDL.ChainIkSolverPos_NR(
self.arm_chain, self.forward_kinematics_solver, self.IK_v_solver)
def load_chain_from_URDF_string_and_joints(urdf_string,
joint_list,
target_robot=False):
"""
This function returns a kinmatic chain based on the URDF string given
and the list of joints
"""
robot = URDF.from_xml_string(urdf_string)
# check if joint is in it if not then expand both in child or parent
base_link, end_link = "", ""
for joint in joint_list:
base_link, end_link = find_links_from_joints(joint,
robot,
base_link,
end_link,
target_robot=target_robot)
urdf_logger.debug("Current base_link:{0}, Current end_link:{1}".format(
base_link, end_link))
# return chain
if base_link and end_link:
tree = kdl_tree_from_urdf_model(robot)
return tree.getChain(base_link, end_link)
else:
return None
def main():
parser = argparse.ArgumentParser(usage='Load an URDF file')
parser.add_argument('file',
type=argparse.FileType('r'),
nargs='?',
default=None,
help='File to load. Use - for stdin')
parser.add_argument('-o',
'--output',
type=argparse.FileType('w'),
default=None,
help='Dump file to XML')
args = parser.parse_args()
if args.file is None:
print 'FROM PARAM SERVER'
robot = URDF.from_parameter_server()
else:
print 'FROM STRING'
robot = URDF.from_xml_string(args.file.read())
print(robot)
if args.output is not None:
args.output.write(robot.to_xml_string())
def parse_model(model_path):
# 1. Parse a string containing the robot description in urdf.
# Pro: no need to have a roscore running.
# Cons: n/a
# Note: it is rare to receive the robot model as a string.
desc = open(model_path, 'r')
robot = URDF.from_xml_string(desc.read())
# - OR -
# 2. Load the module from a file.
# Pro: no need to have a roscore running.
# Cons: using hardcoded file location is not portable.
#robot = urdf.from_xml_file()
# - OR -
# 3. Load the module from the parameter server.
# Pro: automatic, no arguments are needed, consistent
# with other ROS nodes.
# Cons: need roscore to be running and the parameter to
# to be set beforehand (through a roslaunch file for
# instance).
#robot = urdf.from_parameter_server()
# Print the robot
# for element in robot.links:
# print(element)
print(get_all_revolute_joints_names(robot))
return robot
def __init__(self ,urdf=None):
if urdf is None:
print 'FROM PARAM SERVER'
self._youbot = URDF.from_parameter_server(key='robot_description')
else:
print 'FROM STRING'
self._youbot = URDF.from_xml_string(urdf)
self._kdl_tree = kdl_tree_from_urdf_model(self._youbot)
self._base_link = 'arm_link_0'
print "ROOT : ",self._base_link
self._tip_link = 'arm_link_5'
print "self._tip_link : ", self._tip_link
self._tip_frame = PyKDL.Frame()
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
# Baxter Interface Limb Instances
#self._limb_interface = youbot_interface.Limb(limb)
#self._joint_names = self._limb_interface.joint_names()
self._joint_names = ['arm_joint_1', 'arm_joint_2', 'arm_joint_3', 'arm_joint_4', 'arm_joint_5']
self._num_jnts = len(self._joint_names)
# KDL Solvers
self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain)
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain,
self._fk_p_kdl,
self._ik_v_kdl)
self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain)
self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain,
PyKDL.Vector.Zero())
def publish_world_to_base_transform():
while not rospy.has_param('/robot_description'):
rospy.sleep(0.5)
rospy.loginfo("waiting for robot_description")
urdf_str = rospy.get_param('/robot_description')
robot_urdf = URDF.from_xml_string(urdf_str)
robot = URDF.from_xml_string(urdf_str)
robot_root = robot.get_root()
if robot_root is not None and robot_root != "world":
rate = rospy.Rate(10)
while not rospy.is_shutdown():
br = tf.TransformBroadcaster()
br.sendTransform((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0),
rospy.Time.now(), robot_root, "world")
rate.sleep()
def __init__(self):
"""
this class controls and performs mvt of gripper
"""
self.giskard = GiskardWrapper()
rospy.logout("--> Set kitchen/world in Giskard")
rospy.logout("- Set pose kitchen")
kitchen_pose = tf_wrapper.lookup_pose("map", "iai_kitchen/world")
kitchen_pose.header.frame_id = "map"
# setup kitchen
rospy.logout("- Get urdf")
self.urdf = rospy.get_param("kitchen_description")
self.parsed_urdf = URDF.from_xml_string(self.urdf)
self.config_file = None
rospy.logout("- clear urdf and add kitchen urdf")
self.giskard.clear_world()
self.giskard.add_urdf(name="kitchen", urdf=self.urdf, pose=kitchen_pose, js_topic="kitchen/cram_joint_states")
# setup gripper as service
rospy.logout("- Set right and left Gripper service proxy")
self.l_gripper_service = rospy.ServiceProxy('/l_gripper_simulator/set_joint_states', SetJointState)
self.r_gripper_service = rospy.ServiceProxy('/r_gripper_simulator/set_joint_states', SetJointState)
# setup gripper as actionlib
# self.rgripper = actionlib.SimpleActionClient('r_gripper/gripper_command', GripperCommandAction)
# self.lgripper = actionlib.SimpleActionClient('l_gripper/gripper_command', GripperCommandAction)
# self.rgripper.wait_for_server()
# self.lgripper.wait_for_server()
rospy.logout("--> Gripper are ready for every task.")
def create_kdl_kin(base_link, end_link, urdf_filename=None):
if urdf_filename is None:
robot = URDF.from_parameter_server()
else:
f = open(urdf_filename, "r")
robot = URDF.from_xml_string(f.read())
return KDLKinematics(robot, base_link, end_link)
def urdf_to_payload_target(xml_str):
urdf_robot = URDF.from_xml_string(xml_str)
assert len(urdf_robot.link_map
) == 1, "Multiple payload targets detected, invalid URDF."
assert len(urdf_robot.joint_map
) == 1, "Multiple payload targets detected, invalid URDF."
payload_target_link = urdf_robot.link_map.itervalues().next()
payload_target_joint = urdf_robot.joint_map.itervalues().next()
assert payload_target_joint.child == payload_target_link.name, "Invalid payload target URDF"
assert re.match(r'^\w+_target(?:_(\d+))?$',payload_target_link.name) is not None, \
"Invalid payload target name in URDF file"
payload_msg = PayloadTarget()
payload_msg.name = payload_target_link.name
#Load in initial pose
payload_msg.header.frame_id = payload_target_joint.parent
payload_msg.pose = _origin_to_pose(payload_target_joint.origin)
payload_msg.confidence = 0.1
#Sanity check
payload_msg._check_types()
return payload_msg
def __init__(self, urdf_str):
self.urdf_tree = URDF.from_xml_string(urdf_str)
base_link = "link0"
nlinks = len(self.urdf_tree.link_map.keys())
end_link = "link{}".format(nlinks - 1)
print("Link chain goes from {} to {}".format(base_link, end_link))
self.kdl_kin = KDLKinematics(self.urdf_tree, base_link, end_link)
self.ik_solver = IK(base_link, end_link, urdf_string=urdf_str)
def __init__(self):
urdf_file = open(
"/home/shamil/ros/leg_ws/src/leg_description/urdf/leg_setup.urdf",
"r")
self.urdf = URDF.from_xml_string(urdf_file.read())
self.world_to_base_joint_no = 4
self.Twb = self.getWorldToBaseTransform()
self.Tpj = self.getParentToJointTransforms()
def __init__(self):
super(RobotHeadWidget, self).__init__()
self.lock = Lock()
try:
robot_model = URDF.from_xml_string(rospy.get_param("/robot_description"))
except Exception, e:
self.showError("Failed to load /robot_description: " + e.message)
return
def __init__(self, hebiros_wrapper, urdf_str, base_link, end_link):
"""SMACH State
:type hebiros_wrapper: HebirosWrapper
:param hebiros_wrapper: HebirosWrapper instance for Leg HEBI group
:type urdf_str: str
:param urdf_str: Serialized URDF str
:type base_link: str
:param base_link: Leg base link name in URDF
:type end_link: str
:param end_link: Leg end link name in URDF
"""
State.__init__(self,
outcomes=['ik_failed', 'success'],
input_keys=[
'prev_joint_pos', 'target_end_link_point',
'execution_time'
],
output_keys=['prev_joint_pos', 'active_joints'])
self.hebi_wrap = hebiros_wrapper
self.urdf_str = urdf_str
self.base_link = base_link
self.end_link = end_link
self.active = False
# hardware interface
self._hold_leg_position = True
self._hold_joint_angles = []
self.hebi_wrap.add_feedback_callback(self._hold_leg_pos_cb)
# pykdl
self.kdl_fk = KDLKinematics(URDF.from_xml_string(urdf_str), base_link,
end_link)
self._active_joints = self.kdl_fk.get_joint_names()
# trac-ik
self.trac_ik = IK(base_link,
end_link,
urdf_string=urdf_str,
timeout=0.01,
epsilon=1e-4,
solve_type="Distance")
self.ik_pos_xyz_bounds = [0.01, 0.01, 0.01]
self.ik_pos_wxyz_bounds = [31416.0, 31416.0, 31416.0
] # NOTE: This implements position-only IK
# joint state publisher
while not rospy.is_shutdown() and len(
self.hebi_wrap.get_joint_positions()) < len(
self.hebi_wrap.hebi_mapping):
rospy.sleep(0.1)
self._joint_state_pub = rospy.Publisher('joint_states',
JointState,
queue_size=1)
self.hebi_wrap.add_feedback_callback(self._joint_state_cb)
def __init__(self, sim=True):
f = file('/home/hirolab/catkin_ws/src/vspgrid/urdf/widowx2.urdf', 'r')
robot = URDF.from_xml_string(f.read()) # parsed URDF
# robot = URDF.from_parameter_server()
self.kin = KDLKinematics(robot, 'base', 'mconn')
# Selection of end-effector parameters for WidowX
# x, y, z, yaw, pitch
self.cart_from_matrix = lambda T: np.array([
T[0, 3],
T[1, 3],
T[2, 3],
math.atan2(T[1, 0], T[0, 0]),
-math.asin(T[2, 0])])
self.home = np.array([0.05, 0, 0.25, 0, 0]) # home end-effector pose
self.q = np.array([0, 0, 0, 0, 0]) # joint angles
self.dt = 0.05 # algorithm time step
self.sim = sim # true if virtual robot
def publish(eventStop):
# for arbotix
pub1 = rospy.Publisher("q1/command", Float64, queue_size=5)
pub2 = rospy.Publisher("q2/command", Float64, queue_size=5)
pub3 = rospy.Publisher("q3/command", Float64, queue_size=5)
pub4 = rospy.Publisher("q4/command", Float64, queue_size=5)
pub5 = rospy.Publisher("q5/command", Float64, queue_size=5)
# for visualization
jointPub = rospy.Publisher(
"/joint_states", JointState, queue_size=5)
jmsg = JointState()
jmsg.name = ['q1', 'q2', 'q3', 'q4', 'q5']
while not rospy.is_shutdown() and not eventStop.is_set():
jmsg.header.stamp = rospy.Time.now()
jmsg.position = self.q
if self.sim:
jointPub.publish(jmsg)
pub1.publish(self.q[0])
pub2.publish(self.q[1])
pub3.publish(self.q[2])
pub4.publish(self.q[3])
pub5.publish(self.q[4])
eventStop.wait(self.dt)
rospy.init_node("robot")
eventStop = threading.Event()
threadJPub = threading.Thread(target=publish, args=(eventStop,))
threadJPub.daemon = True
threadJPub.start() # Update joint angles in a background process
def __init__(self):
super(RobotHeadWidget, self).__init__()
self.lock = Lock()
try:
robot_model = URDF.from_xml_string(
rospy.get_param("/robot_description"))
except Exception, e:
self.showError("Failed to load /robot_description: " + e.message)
return
def __init__(self, sim=True):
f = file('/home/hirolab/catkin_ws/src/vspgrid/urdf/widowx2.urdf', 'r')
robot = URDF.from_xml_string(f.read()) # parsed URDF
# robot = URDF.from_parameter_server()
self.kin = KDLKinematics(robot, 'base', 'mconn')
# Selection of end-effector parameters for WidowX
# x, y, z, yaw, pitch
self.cart_from_matrix = lambda T: np.array([
T[0, 3], T[1, 3], T[2, 3],
math.atan2(T[1, 0], T[0, 0]), -math.asin(T[2, 0])
])
self.home = np.array([0.05, 0, 0.25, 0, 0]) # home end-effector pose
self.q = np.array([0, 0, 0, 0, 0]) # joint angles
self.dt = 0.05 # algorithm time step
self.sim = sim # true if virtual robot
def publish(eventStop):
# for arbotix
pub1 = rospy.Publisher("q1/command", Float64, queue_size=5)
pub2 = rospy.Publisher("q2/command", Float64, queue_size=5)
pub3 = rospy.Publisher("q3/command", Float64, queue_size=5)
pub4 = rospy.Publisher("q4/command", Float64, queue_size=5)
pub5 = rospy.Publisher("q5/command", Float64, queue_size=5)
# for visualization
jointPub = rospy.Publisher("/joint_states",
JointState,
queue_size=5)
jmsg = JointState()
jmsg.name = ['q1', 'q2', 'q3', 'q4', 'q5']
while not rospy.is_shutdown() and not eventStop.is_set():
jmsg.header.stamp = rospy.Time.now()
jmsg.position = self.q
if self.sim:
jointPub.publish(jmsg)
pub1.publish(self.q[0])
pub2.publish(self.q[1])
pub3.publish(self.q[2])
pub4.publish(self.q[3])
pub5.publish(self.q[4])
eventStop.wait(self.dt)
rospy.init_node("robot")
eventStop = threading.Event()
threadJPub = threading.Thread(target=publish, args=(eventStop, ))
threadJPub.daemon = True
threadJPub.start() # Update joint angles in a background process
def parse_arm_collisions(self, manipulator):
comment = [manipulator.arm.internal_name + " collisions"]
self.add_comments(comment)
previous = self.arm_srdf_xml.documentElement
elt = next_element(previous)
while elt:
if elt.tagName == 'disable_collisions':
elt.getAttributeNode("link1").nodeValue = (
manipulator.arm.prefix + elt.getAttribute("link1"))
elt.getAttributeNode("link2").nodeValue = (
manipulator.arm.prefix + elt.getAttribute("link2"))
elt.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
newElement = deepcopy(elt)
previous = elt
elt = next_element(previous)
# Add collisions between arm and hand
if manipulator.has_hand:
if rospy.has_param('/robot_description'):
urdf_str = rospy.get_param('/robot_description')
robot = URDF.from_xml_string(urdf_str)
arm_chain = robot.get_chain("world",
manipulator.hand.prefix +
"forearm",
joints=False,
fixed=False)
newElement.getAttributeNode("link1").nodeValue = arm_chain[-2]
newElement.getAttributeNode(
"link2").nodeValue = manipulator.hand.prefix + "forearm"
newElement.getAttributeNode("reason").nodeValue = "Adjacent"
newElement.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
newElement.getAttributeNode("link1").nodeValue = arm_chain[-3]
newElement.getAttributeNode(
"link2").nodeValue = manipulator.hand.prefix + "forearm"
newElement.getAttributeNode("reason").nodeValue = "Adjacent"
newElement.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
newElement.getAttributeNode("link1").nodeValue = arm_chain[-4]
newElement.getAttributeNode(
"link2").nodeValue = manipulator.hand.prefix + "forearm"
newElement.getAttributeNode("reason").nodeValue = "Adjacent"
newElement.writexml(self.new_robot_srdf,
indent=" ",
addindent=" ",
newl="\n")
def init_transforms_from_urdf(self):
"""
This static function gets with the robot name in robot_description.py, the
robots URDF file from the "resources" folder in the ROS "pycram" folder.
All joints of the URDF are extracted and saved in a list of static and
normal TFStamped.
:returns: list of static_tf_stampeds, list of tf_stampeds
"""
# Save joint translations and orientations in tf_stampeds
# static_tf_stampeds saves only the static joint translations and orientations
self.tf_stampeds = []
self.static_tf_stampeds = []
# Get URDF file path
robot_name = robot_description.i.name
rospack = rospkg.RosPack()
filename = rospack.get_path(
'pycram') + '/resources/' + robot_name + '.urdf'
# ... and open it
with open(filename) as f:
s = f.read()
robot = URDF.from_xml_string(s)
# Save for every joint in the robot the source_frame, target_frame,
# aswell the joints origin, where the translation xyz and rotation rpy
# are saved
for joint in robot.joints:
source_frame = self.projection_namespace + '/' + joint.parent
target_frame = self.projection_namespace + '/' + joint.child
if joint.origin:
if joint.origin.xyz:
translation = joint.origin.xyz
else:
translation = [0, 0, 0]
if joint.origin.rpy:
roll = joint.origin.rpy[0]
pitch = joint.origin.rpy[1]
yaw = joint.origin.rpy[2]
rotation = transformations.quaternion_from_euler(
roll, pitch, yaw)
else:
rotation = [0, 0, 0, 1]
if joint.name in robot_description.i.odom_joints:
# since pybullet wont update this joints, these are declared as static
translation = [0, 0, 0]
rotation = [0, 0, 0, 1]
# Wrap the joint attributes in a TFStamped and append it to static_tf_stamped if the joint was fixed
tf_stamped = helper.list2tfstamped(source_frame, target_frame,
[translation, rotation])
if (joint.type and joint.type == 'fixed'
) or joint.name in robot_description.i.odom_joints:
self.static_tf_stampeds.append(tf_stamped)
else:
self.tf_stampeds.append(tf_stamped)
def __init__(self, mapping, joint_prefix):
"""
"""
self.joints = {}
hand_joints = []
joints = self.get_default_joints()
TIMEOUT_WAIT_FOR_PARAMS_IN_SECS = 60.0
start_time = rospy.get_time()
while not rospy.has_param("/robot_description"):
if (rospy.get_time() - start_time >
TIMEOUT_WAIT_FOR_PARAMS_IN_SECS):
rospy.logwarn("No robot_description found on parameter server."
"Joint names are loaded for 5 finger hand")
# concatenate all the joints with prefixes
for hand in mapping:
hand_joints = []
if hand in joint_prefix:
for joint in joints:
hand_joints.append(joint_prefix[hand] + joint)
else:
rospy.logwarn("Cannot find serial " + hand +
"in joint_prefix parameters")
self.joints[mapping[hand]] = hand_joints
return
robot_description = rospy.get_param('robot_description')
# concatenate all the joints with prefixes
for hand in mapping:
if hand in joint_prefix:
for joint in joints:
hand_joints.append(joint_prefix[hand] + joint)
else:
rospy.logwarn("Cannot find serial " + hand +
"in joint_prefix parameters")
# add the prefixed joints to each hand but remove fixed joints
hand_urdf = URDF.from_xml_string(robot_description)
for hand in mapping:
joints_tmp = []
self.joints[mapping[hand]] = []
for joint in hand_urdf.joints:
if joint.type != 'fixed':
prefix = joint.name[:3]
# is there an empty prefix ?
if "" in list(joint_prefix.values()):
joints_tmp.append(joint.name)
elif prefix not in list(joint_prefix.values()):
rospy.logdebug("joint " + joint.name + " has invalid "
"prefix:" + prefix)
elif prefix == joint_prefix[hand]:
joints_tmp.append(joint.name)
for joint_unordered in hand_joints:
if joint_unordered in joints_tmp:
self.joints[mapping[hand]].append(joint_unordered)
def from_dict(cls, init_dict):
km = GeometryModel()
urdf = urdf_filler(URDF.from_xml_string(rospy.get_param('/robot_description')))
load_urdf(km, Path(urdf.name), urdf)
km.clean_structure()
km.dispatch_events()
base_frame = init_dict['reference_frame']
eefs = [Endeffector.from_dict(km.get_data(Path(urdf.name)), base_frame, d) for d in init_dict['links']]
return cls(km, Path(urdf.name), eefs)
def loadParams(self):
#Load robot model
self.robot_urdf = URDF.from_xml_string(
rospy.get_param('robot_description'))
#Load maximum number of poses in actual path
self.max_poses = rospy.get_param('~max_poses', 1000)
#Load threshold for adding a pose to actual path
self.threshold = rospy.get_param('~movement_threshold', 0.001)
#Loading tracked joint name
self.tracked_link = rospy.get_param('~tracked_link', 'end_effector')
def urdf_to_link_markers(xml_str):
urdf_robot = URDF.from_xml_string(xml_str)
urdf_et = ET.fromstring(xml_str)
parent_link = None
for _, j in urdf_robot.joint_map.items():
if parent_link is None:
parent_link = j.parent
else:
assert j.parent == parent_link, "Invalid LinkMarkers URDF, all joints must have same parent link"
for _, l in urdf_robot.link_map.items():
assert urdf_robot.parent_map[l.name][1]==parent_link, \
"Invalid LinkMarkers URDF, all links must have same parent link"
payload_msg = LinkMarkers()
payload_msg.header.frame_id = parent_link
#Load in markers
for _, l in urdf_robot.link_map.items():
m = re.match(r'^\w+_marker(?:_(\d+))?$', l.name)
assert m is not None, "Invalid LinkMarkers URDF, all links must contain Aruco tag"
j = urdf_robot.joint_map[urdf_robot.parent_map[l.name][0]]
pose = _origin_to_pose(j.origin)
aruco_marker = ArucoGridboardWithPose()
aruco_marker.header.frame_id = parent_link
aruco_marker.name = l.name
aruco_marker.pose = pose
link_et = urdf_et.find('.//link[@name="' + l.name + '"]')
marker_et = link_et.find('.//aruco_marker')
if marker_et is not None:
gridboard_et = marker_et.find('.//gridboard')
if gridboard_et is not None:
a = gridboard_et.attrib
aruco_marker.marker.markersX = int(a['markersX'])
aruco_marker.marker.markersY = int(a['markersY'])
aruco_marker.marker.markerLength = float(a['markerLength'])
aruco_marker.marker.markerSpacing = float(a['markerSpacing'])
aruco_marker.marker.dictionary = a['dictionary']
aruco_marker.marker.firstMarker = int(a['firstMarker'])
payload_msg.markers.append(aruco_marker)
return payload_msg
def __init__(self):
print("MotionTaskWithConstraint is initialized !")
self._giskard = GiskardWrapper()
rospy.logout("- Set pose kitchen")
kitchen_pose = tf_wrapper.lookup_pose("map", "iai_kitchen/world")
kitchen_pose.header.frame_id = "map"
# setup kitchen
rospy.logout("- Get urdf")
self.urdf = rospy.get_param("kitchen_description")
self.parsed_urdf = URDF.from_xml_string(self.urdf)
self.config_file = None
rospy.logout("- clear urdf and add kitchen urdf")
self._giskard.clear_world()
self._giskard.add_urdf(name="kitchen", urdf=self.urdf, pose=kitchen_pose, js_topic="kitchen/cram_joint_states")
def from_urdf(cls, urdf_string, transmission_suffix='_thruster_transmission'):
'''
Load from an URDF string. Expects each thruster to be connected a transmission ending in the specified suffix.
A transform between the propeller joint and base_link must be available
'''
urdf = URDF.from_xml_string(urdf_string)
buff = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(buff) # noqa
names = []
joints = []
positions = []
angles = []
limit = -1
ratio = -1
for transmission in urdf.transmissions:
find = transmission.name.find(transmission_suffix)
if find != -1 and find + len(transmission_suffix) == len(transmission.name):
if len(transmission.joints) != 1:
raise Exception('Transmission {} does not have 1 joint'.format(transmission.name))
if len(transmission.actuators) != 1:
raise Exception('Transmission {} does not have 1 actuator'.format(transmission.name))
t_ratio = transmission.actuators[0].mechanicalReduction
if ratio != -1 and ratio != t_ratio:
raise Exception('Transmission {} has a different reduction ratio (not supported)'.format(t_ratio))
ratio = t_ratio
joint = None
for t_joint in urdf.joints:
if t_joint.name == transmission.joints[0].name:
joint = t_joint
if joint is None:
rospy.logerr('Transmission joint {} not found'.format(transmission.joints[0].name))
try:
trans = buff.lookup_transform('base_link', joint.child, rospy.Time(), rospy.Duration(10))
except tf2_ros.TransformException as e:
raise Exception(e)
translation = rosmsg_to_numpy(trans.transform.translation)
rot = rosmsg_to_numpy(trans.transform.rotation)
yaw = euler_from_quaternion(rot)[2]
names.append(transmission.name[0:find])
positions.append(translation[0:2])
angles.append(yaw)
joints.append(joint.name)
if limit != -1 and joint.limit.effort != limit:
raise Exception('Thruster {} had a different limit, cannot proceed'.format(joint.name))
limit = joint.limit.effort
return cls(names, positions, angles, ratio, limit, joints=joints)
def __init__(self):
"""
Parses the parameter server to extract the necessary information.
"""
if not rospy.has_param("arm"):
rospy.logerr("No arm param defined.")
arm_parameters = {'joint_prefix': {}, 'mapping': {}}
else:
arm_parameters = rospy.get_param("arm")
# TODO(@anyone): This parameter is never set. This script needs to be modified to find available
# arms from robot_description and present them appropriately. (sr_core issue #74)
self.arm_config = ArmConfig(arm_parameters["mapping"],
arm_parameters["joint_prefix"])
self.arm_joints = {}
if rospy.has_param('robot_description'):
robot_description = rospy.get_param('robot_description')
robot_urdf = URDF.from_xml_string(robot_description)
hand_default_joints = HandJoints.get_default_joints()
possible_arm_joints = []
for joint in robot_urdf.joints:
if joint.type != 'fixed' and joint.name not in hand_default_joints:
match_suffix = False
for hand_default_joint_name in hand_default_joints:
if joint.name.endswith('_' + hand_default_joint_name):
match_suffix = True
break
if not match_suffix:
possible_arm_joints.append(joint.name)
for arm_id, arm_mapping in self.arm_config.mapping.items():
self.arm_joints[arm_mapping] = []
arm_joint_prefix = self.arm_config.joint_prefix[arm_id]
for joint_name in possible_arm_joints:
if joint_name.startswith(arm_joint_prefix):
self.arm_joints[arm_mapping].append(joint_name)
else:
rospy.logwarn(
"No robot_description found on parameter server. Assuming that there is no arm."
)
def configure_robot(self, description):
self.get_logger().info('Got description, configuring robot')
# Load description
# From https://github.com/ros-controls/ros_controllers/blob/noetic-devel/diff_drive_controller/src/diff_drive_controller.cpp
robot = URDF.from_xml_string(description)
# Get left joint wheel
joint_left = get_joint(robot, self.left_wheel_name)
# Get right joint wheel
joint_right = get_joint(robot, self.right_wheel_name)
# Measure wheel separation
self.wheel_separation = euclidean_of_vectors(joint_left.origin.xyz,
joint_right.origin.xyz)
# Get radius joint
link_left = get_link(robot, joint_left.child)
self.radius = link_left.collision.geometry.radius
self.get_logger().info(
f"Wheel separation {self.wheel_separation} - Radius {self.radius}")
def xacro(self, file, args=''):
"""
Generates a URDF from a XACRO file. If the URDF is invalid the test is
automatically failed.
@param file: The name of the xacro input file within `franka_description/robots/`
@param args: A optional string of xacro args to append, e.g. ("foo:=1 bar:=true")
@return: The generated URDF, as urdf_parser_py.urdf.URDF type
"""
try:
return URDF.from_xml_string(
subprocess.check_output(
'xacro $(rospack find %s)/robots/%s %s' %
(PKG, file, args),
shell=True))
except subprocess.CalledProcessError as e:
self.fail(
'Could not generate URDF from "%s", probably syntax error: %s'
% (file, e.output))
def __init__(self, urdf, default_joint_vel_limit=0):
"""
:param urdf:
:type urdf: str
:param joints_to_symbols_map: maps urdf joint names to symbols
:type joints_to_symbols_map: dict
:param default_joint_vel_limit: all velocity limits which are undefined or higher than this will be set to this
:type default_joint_vel_limit: float
"""
self.default_joint_velocity_limit = default_joint_vel_limit
self.default_weight = 0.0001
self.fks = {}
self._joint_to_frame = {}
self.joint_to_symbol_map = keydefaultdict(lambda x: spw.Symbol(x))
self.urdf = urdf
with suppress_stderr():
self._urdf_robot = URDF.from_xml_string(
hacky_urdf_parser_fix(self.urdf))
def main():
parser = argparse.ArgumentParser(usage='Load an URDF file')
parser.add_argument('file', type=argparse.FileType('r'), nargs='?',
default=None, help='File to load. Use - for stdin')
parser.add_argument('-o', '--output', type=argparse.FileType('w'),
default=None, help='Dump file to XML')
args = parser.parse_args()
if args.file is None:
print 'FROM PARAM SERVER'
robot = URDF.from_parameter_server()
else:
print 'FROM STRING'
robot = URDF.from_xml_string(args.file.read())
print(robot)
if args.output is not None:
args.output.write(robot.to_xml_string())
def __init__(self):
self.goal_traj = []
self.real_traj = []
self.acc_limit = PyKDL.Twist()
self.acc_limit.vel[0] = 0.01
self.acc_limit.vel[1] = 0.01
self.acc_limit.rot[2] = 0.01
self.done = True
urdf = rospy.get_param('robot_description')
self.urdf = URDF.from_xml_string(hacky_urdf_parser_fix(urdf))
limits = PyKDL.Twist()
limits.vel[0] = self.urdf.joint_map[x_joint].limit.velocity
limits.vel[1] = self.urdf.joint_map[y_joint].limit.velocity
limits.rot[2] = self.urdf.joint_map[z_joint].limit.velocity
self._min_vel = -limits
self._max_vel = limits
self.cmd = None
self.last_cmd = PyKDL.Twist()
self.pose_history = None
self.cmd_vel_sub = rospy.Publisher('~cmd_vel', Twist, queue_size=10)
self.debug_vel = rospy.Publisher('debug_vel', Twist, queue_size=10)
js = rospy.wait_for_message('/joint_states', JointState)
self.x_index_js = js.name.index(x_joint)
self.y_index_js = js.name.index(y_joint)
self.z_index_js = js.name.index(z_joint)
self.current_goal = None
self.js_sub = rospy.Subscriber('/joint_states',
JointState,
self.js_cb,
queue_size=10)
self.state_pub = rospy.Publisher('{}/state'.format(name_space),
JointTrajectoryControllerState,
queue_size=10)
self.server = SimpleActionServer(
'{}/follow_joint_trajectory'.format(name_space),
FollowJointTrajectoryAction,
self.execute_cb,
auto_start=False)
self.server.start()
def execute(self, ud):
self.enter(ud)
urdf_str = ud.urdf_str
urdf = URDF.from_xml_string(urdf_str)
transforms = []
# pykdl
kdl_list = []
for base_link in self.base_links:
for end_link in self.end_links:
kdl = KDLKinematics(urdf, base_link, end_link)
kdl_list.append(kdl)
transform = kdl.forward([])
transforms.append(transform)
ud.transforms = transforms
return self.exit(ud, 'done')
def from_ros_params(cls):
'''
Initialize class from URDF and /robot_paramters/ ros param set by upload.launch
'''
# Parse URDF of Sub for inertial information, etc
urdf_string = rospy.get_param('/robot_description')
urdf = URDF.from_xml_string(urdf_string)
# Get root link of model
root = urdf.link_map[urdf.get_root()]
mass = root.inertial.mass
rotational_inertia = np.array(root.inertial.inertia.to_matrix())
drag_linear = np.array(rospy.get_param('/robot_parameters/drag/linear_coeffs'))
drag_angular = np.array(rospy.get_param('/robot_parameters/drag/angular_coeffs'))
drag_coeffs = np.hstack((drag_linear, drag_angular)).T
volume = rospy.get_param('/robot_parameters/volume')
height = rospy.get_param('/robot_parameters/height')
water_density = rospy.get_param('/robot_parameters/fluid_density')
air_density = rospy.get_param('/robot_parameters/air_density')
G = rospy.get_param('/robot_parameters/G')
return cls(mass, rotational_inertia, volume, drag_coeffs, height,
water_density, air_density, G)
class BarrettDashboard(Plugin):
def __init__(self, context):
super(BarrettDashboard, self).__init__(context)
self._joint_sub = None
# Give QObjects reasonable names
self.setObjectName('BarrettDashboard')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'barrett_dashboard.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('BarrettDashboardPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self.context = context
jp_widgets = [getattr(self._widget,'jp_%d' % i) for i in range(7)]
jn_widgets = [getattr(self._widget,'jn_%d' % i) for i in range(7)]
self.joint_widgets = zip(jp_widgets,jn_widgets)
tp_widgets = [getattr(self._widget,'tp_%d' % i) for i in range(7)]
tn_widgets = [getattr(self._widget,'tn_%d' % i) for i in range(7)]
self.torque_widgets = zip(tp_widgets,tn_widgets)
self.joint_signals = []
self.torque_signals = []
for (tp,tn) in self.torque_widgets:
tp.setRange(0.0,1.0,False)
tn.setRange(1.0,0.0,False)
tp.setValue(0.0)
tn.setValue(0.0)
# set random values for testing
#v = (2.0*random.random()) - 1.0
#tp.setValue(v if v >=0 else 0)
#tn.setValue(-v if v <0 else 0)
for (jp,jn) in self.joint_widgets:
jp.setRange(0.0,1.0,False)
jn.setRange(1.0,0.0,False)
jp.setValue(0.0)
jn.setValue(0.0)
# set random values for testing
#v = (2.0*random.random()) - 1.0
#jp.setValue(v if v >=0 else 0)
#jn.setValue(-v if v <0 else 0)
self.barrett_green = QColor(87,186,142)
self.barrett_green_dark = self.barrett_green.darker()
self.barrett_green_light = self.barrett_green.lighter()
self.barrett_blue = QColor(80,148,204)
self.barrett_blue_dark = self.barrett_blue.darker()
self.barrett_blue_light = self.barrett_blue.lighter()
self.barrett_red = QColor(232,47,47)
self.barrett_red_dark = self.barrett_red.darker()
self.barrett_red_light = self.barrett_red.lighter()
self.barrett_orange = QColor(255,103,43)
self.barrett_orange_dark = self.barrett_orange.darker()
#background_color = p.mid().color()
joint_bg_color = self.barrett_blue_dark.darker()
joint_fill_color = self.barrett_blue
joint_alarm_color = self.barrett_blue #self.barrett_blue_light
torque_bg_color = self.barrett_green_dark.darker()
torque_fill_color = self.barrett_green
torque_alarm_color = self.barrett_orange #self.barrett_green_light
temp_bg_color = self.barrett_red_dark.darker()
temp_fill_color = self.barrett_orange
temp_alarm_color = self.barrett_red
for w in jp_widgets + jn_widgets:
w.setAlarmLevel(0.80)
w.setFillColor(joint_fill_color)
w.setAlarmColor(joint_alarm_color)
p = w.palette()
p.setColor(tp.backgroundRole(), joint_bg_color)
w.setPalette(p)
for w in tp_widgets + tn_widgets:
w.setAlarmLevel(0.66)
w.setFillColor(torque_fill_color)
w.setAlarmColor(torque_alarm_color)
p = w.palette()
p.setColor(tp.backgroundRole(), torque_bg_color)
w.setPalette(p)
self._widget.hand_temp.setAlarmLevel(0.66)
self._widget.hand_temp.setFillColor(temp_fill_color)
self._widget.hand_temp.setAlarmColor(temp_alarm_color)
p = self._widget.hand_temp.palette()
p.setColor(self._widget.hand_temp.backgroundRole(), temp_bg_color)
self._widget.hand_temp.setPalette(p)
self._widget.hand_temp.setOrientation(Qt.Horizontal, QwtThermo.RightScale)
self._widget.jf_0.setStyleSheet("QLabel { background-color : rgb(%d,%d,%d); color : rgb(%d,%d,%d); }" % (
joint_bg_color.red(), joint_bg_color.green(), joint_bg_color.blue(), joint_fill_color.red(), joint_fill_color.green(), joint_fill_color.blue()))
self.urdf = rospy.get_param('robot_description')
self.robot = URDF()
self.robot = self.robot.from_xml_string(self.urdf)
self.pos_norm = [0] * 7
self.torque_norm = [0] * 7
self._joint_sub = rospy.Subscriber(
'joint_states',
sensor_msgs.msg.JointState,
self._joint_state_cb)
self._status_sub = rospy.Subscriber(
'status',
oro_barrett_msgs.msg.BarrettStatus,
self._status_cb)
self._hand_status_sub = rospy.Subscriber(
'hand/status',
oro_barrett_msgs.msg.BHandStatus,
self._hand_status_cb)
self._update_status(-1)
self.safety_mode = -1
self.run_mode = 0
self.hand_run_mode = 0
self.hand_min_temperature = 40.0
self.hand_max_temperature = 65.0
self.hand_temperature = 0.0
self.update_timer = QTimer(self)
self.update_timer.setInterval(50)
self.update_timer.timeout.connect(self._update_widget_values)
self.update_timer.start()
self.set_home_client = actionlib.SimpleActionClient(
'wam/set_home_action',
oro_barrett_msgs.msg.SetHomeAction)
self.set_mode_client = actionlib.SimpleActionClient(
'wam/set_mode_action',
oro_barrett_msgs.msg.SetModeAction)
self._widget.button_set_home.clicked[bool].connect(self._handle_set_home_clicked)
self._widget.button_idle_wam.clicked[bool].connect(self._handle_idle_wam_clicked)
self._widget.button_run_wam.clicked[bool].connect(self._handle_run_wam_clicked)
self.bhand_init_client = actionlib.SimpleActionClient(
'hand/initialize_action',
oro_barrett_msgs.msg.BHandInitAction)
self.bhand_set_mode_client = actionlib.SimpleActionClient(
'hand/set_mode_action',
oro_barrett_msgs.msg.BHandSetModeAction)
self.grasp_client = actionlib.SimpleActionClient(
'grasp', BHandGraspAction)
self.release_client = actionlib.SimpleActionClient(
'release', BHandReleaseAction)
self.spread_client = actionlib.SimpleActionClient(
'spread', BHandSpreadAction)
self._widget.button_initialize_hand.clicked[bool].connect(self._handle_initialize_hand_clicked)
self._widget.button_idle_hand.clicked[bool].connect(self._handle_idle_hand_clicked)
self._widget.button_run_hand.clicked[bool].connect(self._handle_run_hand_clicked)
self._widget.button_release_hand.clicked[bool].connect(self._handle_release_hand_clicked)
self._widget.button_grasp_hand.clicked[bool].connect(self._handle_grasp_hand_clicked)
self._widget.button_set_spread.clicked[bool].connect(self._handle_spread_hand_clicked)
self._widget.resizeEvent = self._handle_resize
def _handle_resize(self, event):
for i,((w1,w2),(w3,w4)) in enumerate(zip(self.joint_widgets, self.torque_widgets)):
width = 0.8*getattr(self._widget, 'jcc_%d'%i).contentsRect().width()
w1.setPipeWidth(width)
w2.setPipeWidth(width)
w3.setPipeWidth(width)
w4.setPipeWidth(width)
def _handle_set_home_clicked(self, checked):
if checked:
self.set_home()
def _update_status(self, status):
if status == -1:
self._widget.safety_mode.setText('UNKNOWN SAFETY MODE')
self._widget.safety_mode.setToolTip('The WAM is in an unknown state. Proceed with caution.')
color = QColor(200,200,200)
self._widget.button_set_home.setEnabled(False)
self._widget.button_idle_wam.setEnabled(False)
self._widget.button_run_wam.setEnabled(False)
elif status == 0:
self._widget.safety_mode.setText('E-STOP')
self._widget.safety_mode.setToolTip('The WAM is stopped and unpowered. Determine the source of the fault and reset the arm to continue using it.')
self._widget.button_set_home.setEnabled(False)
self._widget.button_idle_wam.setEnabled(False)
self._widget.button_run_wam.setEnabled(False)
color = self.barrett_red
else:
if not self.homed:
self._widget.safety_mode.setText('UNCALIBRATED')
self._widget.safety_mode.setToolTip('The WAM is not calibrated. Please place it in the calibration posture and click the "Calibrate" button.')
self._widget.button_set_home.setEnabled(True)
self._widget.button_idle_wam.setEnabled(False)
self._widget.button_run_wam.setEnabled(False)
color = self.barrett_orange
else:
if status == 1:
self._widget.safety_mode.setText('IDLE')
self._widget.safety_mode.setToolTip('The WAM is running but all joints are passive. It is safe to home the arm.')
self._widget.button_set_home.setEnabled(True)
self._widget.button_idle_wam.setEnabled(True)
self._widget.button_run_wam.setEnabled(True)
color = self.barrett_blue
elif status == 2:
self._widget.safety_mode.setText('ACTIVE')
self._widget.safety_mode.setToolTip('The WAM is running and all joints are active. Proceed with caution.')
self._widget.button_set_home.setEnabled(False)
self._widget.button_idle_wam.setEnabled(False)
self._widget.button_run_wam.setEnabled(False)
color = self.barrett_green
darker = color.darker()
lighter = color.lighter()
self._widget.safety_mode.setStyleSheet("QLabel { background-color : rgb(%d,%d,%d); color : rgb(%d,%d,%d); }" % (
color.red(), color.green(), color.blue(), lighter.red(), lighter.green(), lighter.blue()))
def _update_widget_values(self):
if self.safety_mode in (oro_barrett_msgs.msg.SafetyMode.ACTIVE, oro_barrett_msgs.msg.SafetyMode.IDLE):
for (v,(tp,tn)) in zip(self.torque_norm,self.torque_widgets):
tp.setEnabled(True)
tn.setEnabled(True)
tp.setValue(v if v >=0 else 0)
tn.setValue(-v if v <0 else 0)
for (v,(jp,jn)) in zip(self.pos_norm,self.joint_widgets):
jp.setEnabled(True)
jn.setEnabled(True)
jp.setValue(v if v >=0 else 0)
jn.setValue(-v if v <0 else 0)
else:
for (p,n) in self.joint_widgets + self.torque_widgets:
p.setEnabled(True)
n.setEnabled(True)
self._widget.hand_temp.setValue((self.hand_temperature-self.hand_min_temperature)/(self.hand_max_temperature-self.hand_min_temperature))
self._update_status(self.safety_mode)
self._update_buttons(self.run_mode, self.hand_run_mode)
def _update_buttons(self, run_mode, hand_run_mode):
if run_mode == oro_barrett_msgs.msg.RunMode.IDLE:
self._widget.button_idle_wam.setChecked(True)
self._widget.button_run_wam.setChecked(False)
else:
self._widget.button_idle_wam.setChecked(False)
self._widget.button_run_wam.setChecked(True)
if hand_run_mode == oro_barrett_msgs.msg.RunMode.RUN:
self._widget.button_idle_hand.setChecked(False)
self._widget.button_run_hand.setChecked(True)
else:
self._widget.button_idle_hand.setChecked(True)
self._widget.button_run_hand.setChecked(False)
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
#def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget title bar
# Usually used to open a modal configuration dialog
def _joint_state_cb(self, msg):
joint_pos_map = dict(zip(msg.name, msg.position))
for (name,pos,torque,i) in zip(msg.name,msg.position,msg.effort,range(7)):
joint = self.robot.joint_map[name]
self.pos_norm[i] = 2.0*((pos-joint.limit.lower)/(joint.limit.upper-joint.limit.lower)) - 1.0
self.torque_norm[i] = torque/joint.limit.effort
def _status_cb(self, msg):
self.safety_mode = msg.safety_mode.value
self.run_mode = msg.run_mode.value
self.homed = msg.homed
if self.safety_mode == oro_barrett_msgs.msg.SafetyMode.ACTIVE:
self._widget.button_initialize_hand.setEnabled(False)
elif self.safety_mode == oro_barrett_msgs.msg.SafetyMode.ESTOP:
self._widget.button_initialize_hand.setEnabled(False)
self._widget.button_idle_hand.setEnabled(False)
self._widget.button_run_hand.setEnabled(False)
def _hand_status_cb(self, msg):
self.hand_initialized = msg.initialized
self.hand_temperature = max(msg.temperature)
self.hand_run_mode = msg.run_mode.value
def _handle_set_home_clicked(self, checked):
goal = oro_barrett_msgs.msg.SetHomeGoal()
self.set_home_client.send_goal(goal)
def _handle_idle_wam_clicked(self, checked):
if checked:
goal = oro_barrett_msgs.msg.SetModeGoal()
goal.mode.value = oro_barrett_msgs.msg.RunMode.IDLE
self.set_mode_client.send_goal(goal)
def _handle_run_wam_clicked(self, checked):
if checked:
goal = oro_barrett_msgs.msg.SetModeGoal()
goal.mode.value = oro_barrett_msgs.msg.RunMode.RUN
self.set_mode_client.send_goal(goal)
def _handle_initialize_hand_clicked(self, checked):
if self.safety_mode == oro_barrett_msgs.msg.SafetyMode.IDLE:
goal = oro_barrett_msgs.msg.BHandInitGoal()
self.bhand_init_client.send_goal(goal)
def _handle_run_hand_clicked(self, checked):
if checked:
goal = oro_barrett_msgs.msg.BHandSetModeGoal()
goal.run_mode.value = oro_barrett_msgs.msg.RunMode.RUN
self.bhand_set_mode_client.send_goal(goal)
def _handle_idle_hand_clicked(self, checked):
if checked:
goal = oro_barrett_msgs.msg.BHandSetModeGoal()
goal.run_mode.value = oro_barrett_msgs.msg.RunMode.IDLE
self.bhand_set_mode_client.send_goal(goal)
def _get_grasp_mask(self):
return [ self._widget.button_use_f1.isChecked(),
self._widget.button_use_f2.isChecked(),
self._widget.button_use_f3.isChecked()]
def _handle_release_hand_clicked(self, checked):
goal = BHandReleaseGoal()
goal.release_mask = self._get_grasp_mask()
goal.release_speed = [3.0, 3.0, 3.0]
self.release_client.send_goal(goal)
def _handle_grasp_hand_clicked(self, checked):
goal = BHandGraspGoal()
goal.grasp_mask = self._get_grasp_mask()
goal.grasp_speed = [1.0, 1.0, 1.0]
goal.grasp_effort = [1.0, 1.0, 1.0]
goal.min_fingertip_radius = 0.0
self.grasp_client.send_goal(goal)
def _handle_spread_hand_clicked(self, checked):
goal = BHandSpreadGoal()
goal.spread_position = self._widget.spread_slider.value()/1000.0*3.141
self.spread_client.send_goal(goal)
class Hybrid():
def __init__(self):
rospy.init_node('hybrid_node')
self.freq = 100
self.rate = rospy.Rate(self.freq) # 100 hz
# pub
self.pub_test = rospy.Publisher('/hybrid/test', String)
# sub
self.sub_jr3 = rospy.Subscriber('/jr3/wrench', WrenchStamped, self.cb_jr3)
self.sub_joy = rospy.Subscriber('/spacenav/joy', Joy, self.cb_joy)
self.sub_enable = rospy.Subscriber('/hybrid/enable', Bool, self.cb_enable)
self.sub_cmd = rospy.Subscriber('/hybrid/cmd', String, self.cb_cmd)
# tf
self.ler = tf.TransformListener() # listener
self.ber = tf.TransformBroadcaster() # broadcaster
# datas
self.enabled = False
self.cmdfrm = Frame()
self.wrench = Wrench()
self.cmdtwist = Twist()
self.urdf = rospy.get_param('/wam/robot_description')
print self.urdf
self.robot = URDF()
self.robot = self.robot.from_xml_string(self.urdf)
self.chain = self.robot.get_chain('base_link',
'cutter_tip_link')
print self.chain
pass
def cb_jr3(self, msg):
self.wrench.force = Vector(msg.wrench.force.x,
msg.wrench.force.y,
msg.wrench.force.z)
self.wrench.torque = Vector(msg.wrench.torque.x,
msg.wrench.torque.y,
msg.wrench.torque.z)
pass
def cb_joy(self, msg):
# might want to rotate this twist
# self.cmdtwist.vel = Vector(msg.axes[0], msg.axes[1], msg.axes[2])
# self.cmdtwist.rot = Vector(msg.axes[3], msg.axes[4], msg.axes[5])
# align twist to be same as base link
self.cmdtwist.vel = Vector(msg.axes[2], msg.axes[1], -msg.axes[0])
self.cmdtwist.rot = Vector(msg.axes[5], msg.axes[4], -msg.axes[3])
# print self.cmdtwist
pass
def cb_enable(self, msg):
self.enabled = msg.data
if msg.data:
# sync cmd pos/rot
rospy.loginfo("enable hybrid force control")
try:
(trans, rot) = \
self.ler.lookupTransform('wam/base_link',
'wam/cutter_tip_link',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "tf read error"
self.cmdfrm.p = Vector(trans[0], trans[1], trans[2])
self.cmdfrm.M = Rotation.Quaternion(rot[0], rot[1], rot[2], rot[3])
print self.cmdfrm
pass
def cb_cmd(self, msg):
pass
def ctrl_hybrid(self):
# massage cmd frm with force controller
# z: force control on z axis
cmdforce = -4 # -4N on z axis
errforce = cmdforce - self.wrench.force.z()
print self.wrench.force
pass
def run(self):
while not rospy.is_shutdown():
msg = String()
msg.data = 'yeah'
self.pub_test.publish(msg)
# update states
# option 1: direct
# option 2: hybrid
if self.enabled:
gain_vel = 0.1
gain_rot = 0.5
self.cmdtwist.vel *= gain_vel
self.cmdtwist.rot *= gain_rot
# self.cmdfrm.Integrate(self.cmdtwist, self.freq)
tmptwist = self.cmdfrm.M.Inverse(self.cmdtwist)
self.cmdfrm.Integrate(tmptwist, self.freq)
# hybrid
self.ctrl_hybrid()
cmdtrans = (self.cmdfrm.p.x(), self.cmdfrm.p.y(), self.cmdfrm.p.z())
cmdrot = self.cmdfrm.M.GetQuaternion()
self.ber.sendTransform(cmdtrans,
cmdrot,
rospy.Time.now(),
"wam/cmd",
"wam/base_link")
# print cmdTrans
self.rate.sleep()
text.fg_color.b = color[2] / 255.0
text.fg_color.a = 1.0
text.bg_color.a = 0.0
text.text = "%dC -- (%s)" % (int(max_temparature), max_temparature_name)
g_text_publisher.publish(text)
def jointStatesCallback(msg):
global g_effort_publishers, robot_model
values = msg.effort
names = msg.name
allocateEffortPublishers(names)
for val, name in zip(values, names):
# lookup effort limit
candidate_joints = [j for j in robot_model.joints if j.name == name]
if candidate_joints:
if candidate_joints[0].limit:
limit = candidate_joints[0].limit.effort
else:
limit = 0
if limit != 0:
g_effort_publishers[name].publish(Float32(abs(val/limit)))
if __name__ == "__main__":
rospy.init_node("motor_state_temperature_decomposer")
robot_model = URDF.from_xml_string(rospy.get_param("/robot_description"))
g_text_publisher = rospy.Publisher("max_temparature_text", OverlayText)
s = rospy.Subscriber("/motor_states", MotorStates, motorStatesCallback, queue_size=1)
s_joint_states = rospy.Subscriber("/joint_states", JointState, jointStatesCallback, queue_size=1)
#s = rospy.Subscriber("joint_states", MotorStates, motorStatesCallback)
rospy.spin()
#!/usr/bin/env python
import sys
import argparse
from urdf_parser_py.urdf import URDF
parser = argparse.ArgumentParser(usage='Load an URDF file')
parser.add_argument('file', type=argparse.FileType('r'), nargs='?', default=None, help='File to load. Use - for stdin')
parser.add_argument('-o', '--output', type=argparse.FileType('w'), default=None, help='Dump file to XML')
args = parser.parse_args()
if args.file is None:
print 'VERIFYING FROM PARAM SERVER'
robot = URDF.from_parameter_server()
else:
print 'VERIFYING FROM STRING'
robot = URDF.from_xml_string(args.file.read())
print(robot)
print "ALL IS WELL! URDF PASSED PARSING!"
if args.output is not None:
args.output.write(robot.to_xml_string())
