def get_ik(position):
"""Get the inverse kinematics
Get the inverse kinematics of the UR5 robot using track_IK package giving a desired intial position
Arguments:
position {list} -- A position representing x, y and z
Returns:
sol {list} -- Joint angles or None if track_ik is not able to find a valid solution
"""
camera_support_angle_offset = 0.0
q = quaternion_from_euler(0.0, -3.14 + camera_support_angle_offset, 0.0)
# Joint order:
# ('shoulder_link', 'upper_arm_link', 'forearm_link', 'wrist_1_link', 'wrist_2_link', 'wrist_3_link', 'grasping_link')
ik_solver = IK("base_link", "grasping_link", solve_type="Distance")
sol = ik_solver.get_ik([
0.2201039360819781, -1.573845095552878, -1.521853400505349,
-1.6151347051274518, 1.5704492904506875, 0.0
], position[0], position[1], position[2], q[0], q[1], q[2], q[3])
if sol is not None:
sol = list(sol)
sol[-1] = 0.0
else:
print("IK didn't return any solution")
return sol
class IKSolver():
def __init__(self):
self.ik_solver = IK("world", "link4", timeout=0.1, solve_type="Distance")
self.start_state = [0.0] * (self.ik_solver.number_of_joints - 1)
self.bx = self.by = self.bz = 0.001
self.brx = self.bry = self.brz = 0.1
self.x, self.y, self.z, self.qx, self.qy, self.qz, self.qw = 0, 0, 0, 0, 0, 0, 1
rospy.init_node("ik_solver")
self.ik_solution_pub = rospy.Publisher("/arm/move_jp", JointState, queue_size=10)
self.position_for_controller = JointState()
self.ROS_main_loop()
# lb, up = self.ik_solver.get_joint_limits()
def ik_find_solution(self, x, y, z, qx, qy, qz, qw):
if self.x != x or self.y != y or self.z != z or self.qx != qx or self.qy != qy or self.qz != qz or self.qw != qw:
self.sol = self.ik_solver.get_ik(self.curent_js.position, self.x, self.y, self.z,
self.qx, self.qy, self.qz, self.qw,
self.bx, self.by, self.bz,
self.brx, self.bry, self.brz)
self.x = x
self.y = y
self.z = z
self.qx = qx
self.qy = qy
self.qz = qz
self.qw = qw
if self.sol != None:
self.sol = [round(i, 2) for i in self.sol]
print(self.sol)
self.position_for_controller.position = self.sol
self.position_for_controller.position.append(0.0)
self.ik_solution_pub.publish(self.position_for_controller)
else:
print("No solution")
def ik_callback(self, pose):
x = pose.position.x
y = pose.position.y
z = pose.position.z
qx = pose.orientation.x
qy = pose.orientation.y
qz = pose.orientation.z
qw = pose.orientation.w
print(pose)
self.ik_find_solution(x, y, z, qx, qy, qz, qw)
def js_cb(self, js):
self.curent_js.position = js.position[:-1]
def ROS_main_loop(self):
self.curent_js = JointState()
rospy.Subscriber("/l_cont_pose", Pose, self.ik_callback)
rospy.Subscriber("/arm/measured_js", JointState, self.js_cb)
rospy.loginfo("Init done. IK plugin is working")
rospy.spin()
def trac_ik_solve(limb, ps):
print 'publish', ps
target_topic.publish(ps)
local_base_frame = limb + "_arm_mount"
ik_solver = IK(local_base_frame, limb + "_wrist", urdf_string=urdf_str)
seed_state = [0.0] * ik_solver.number_of_joints
# canonical pose in local_base_frame
#hdr = Header(stamp=rospy.Time.now(), frame_id=from_frame)
#ps = PoseStamped(
# header=hdr,
# pose=pose,
# )
p = translate_frame(ps, local_base_frame)
#print 'translated frame',p
soln = ik_solver.get_ik(
seed_state,
p.pose.position.x,
p.pose.position.y,
p.pose.position.z, # X, Y, Z
p.pose.orientation.x,
p.pose.orientation.y,
p.pose.orientation.z,
p.pose.orientation.w, # QX, QY, QZ, QW
0.01,
0.01,
0.01,
0.1,
0.1,
0.1,
)
print 'trac soln', soln
return soln
class GraspKDL():
'''
Grasp KDL class.
'''
def __init__(self):
# rospy.init_node('grasp_kdl')
self.robot = URDF.from_parameter_server()
base_link = 'world'
end_link = 'palm_link'
self.kdl_kin = KDLKinematics(self.robot, base_link, end_link)
self.ik_solver = IK(base_link, end_link)
self.seed_state = [0.0] * self.ik_solver.number_of_joints
def forward(self, q):
pose = self.kdl_kin.forward(q)
return pose
def jacobian(self, q):
J = self.kdl_kin.jacobian(q)
return J
def inverse(self, pose):
ik_js = self.ik_solver.get_ik(self.seed_state, pose.position.x,
pose.position.y, pose.position.z,
pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
if ik_js is None:
rospy.logerr('No IK solution for grasp planning!')
return None
return ik_js
class RobotKinematics:
"""Calculate direct and inverse kinematics through different methods"""
def __init__(self, from_rosparam=False):
if from_rosparam:
try:
urdf = rospy.get_param('/robot_description')
except:
urdf = open("modello.urdf").read()
else:
urdf = open("modello.urdf").read()
urdf = urdf.replace('encoding="utf-8"', '').replace('encoding=\'utf-8\'', '')
self.chain = kp.build_chain_from_urdf(urdf)
self.njoints = len( self.chain.get_joint_parameter_names() )
self.end_link = "end_effector" # "link{}".format(self.njoints)
self.ik_solver = IK("link0", self.end_link, urdf_string=urdf)
def calculate_direct_kinematic(self, joint_angles):
assert len(joint_angles) == self.njoints
joint_state = {}
for i in range(0, len(joint_angles)):
joint_state["joint{}".format(i)] = joint_angles[i]
all_link_positions = self.chain.forward_kinematics(joint_state)
return all_link_positions[self.end_link].pos
def calculate_inverse_kinematics_ik(self, x, y, z):
initial_state = np.array(self.njoints * [0.0]).astype(float)
return self.ik_solver.get_ik(initial_state, x, y, z, 0, 0, 0, 1.0, 1e-8, 1e-8, 1e-8)
def calculate_inverse_kinematics_optimization(self, x, y, z):
goal_position = np.array([x, y, z])
print("Optimize ", goal_position)
def distance_from_goal(joint_angles):
current_position = self.calculate_direct_kinematic(joint_angles)
return np.linalg.norm(goal_position - current_position)
JOINT_LIMIT = (np.pi - 0.0000001)/4.0
N_JOINTS = self.njoints
bounds = ( (-JOINT_LIMIT, JOINT_LIMIT), ) * N_JOINTS
initial_state = np.array(N_JOINTS * [0.0]).astype(float)
print("From initial state ", initial_state)
solution = minimize(distance_from_goal, initial_state, method='SLSQP', bounds=bounds)
return solution.x
def get_joints_number(self):
return self.njoints
def trac_ik_solve(limb, ps):
try:
arm = baxter_interface.Limb(limb)
state = arm.joint_angles()
print 'solve current:', arm.joint_angles()
jointnames = ['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2']
command = []
for i in range(0, len(jointnames)):
key = limb + "_" + jointnames[i]
command.append(state[key])
print 'candidate seed', command
local_base_frame = limb + "_arm_mount"
ik_solver = IK(
local_base_frame,
limb + "_wrist",
#limb+"_gripper",
urdf_string=urdf_str)
#seed_state = [0.0] * ik_solver.number_of_joints
seed_state = command
# canonical pose in local_base_frame
#hdr = Header(stamp=rospy.Time.now(), frame_id=from_frame)
#ps = PoseStamped(
# header=hdr,
# pose=pose,
# )
#gripper_ps = translate_in_frame(ps,'right_wrist',0,0,0)
#gripper_ps = translate_pose_in_own_frame(ps,'gripper_target',0.015,-0.02,-0.2)
#gripper_ps = translate_pose_in_own_frame(ps,'gripper_target',0,0,0.05)
p = translate_frame(ps, local_base_frame)
#print 'translated frame',p
soln = ik_solver.get_ik(
seed_state,
p.pose.position.x,
p.pose.position.y,
p.pose.position.z, # X, Y, Z
p.pose.orientation.x,
p.pose.orientation.y,
p.pose.orientation.z,
p.pose.orientation.w, # QX, QY, QZ, QW
#0.01,0.01,0.01,
0.1,
0.1,
0.1,
0.3,
0.3,
0.3,
)
print 'trac soln', soln
return soln
except:
return None
def ik_solver(X, Y, Z, QX, QY, QZ, QW):
urdf_str = rospy.get_param('/robot_description')
#print urdf_str
ik_sol = IK("panda_link0","panda_link7",urdf_string=urdf_str)
#print ik_sol.link_names
global position
seed_state = position[2:9]
lower_bound, upper_bound = ik_sol.get_joint_limits()
#print upper_bound
#print lower_bound
ik_sol.set_joint_limits(lower_bound, upper_bound)
return ik_sol.get_ik(seed_state,
X, Y, Z, # X, Y, Z
QX, QY, QZ, QW) # QX, QY, QZ, QW
class ConbeIK():
def __init__(self, urdf_param, LorR, pos_bound, ori_bound):
# Get URDF from parameter server
# self._urdf_str = rospy.get_param('/LArm/robot_description')
self._urdf_str = rospy.get_param(urdf_param)
# Create IK instance
self._ik_solver = IK(LorR + "link0",
LorR + "EEFlink",
urdf_string=self._urdf_str)
# set the Dof
self._seed_state = [0.0] * self._ik_solver.number_of_joints
#set the lower&upper bound
self._lower_bound, self._upper_bound = self._ik_solver.get_joint_limits(
)
#can modify the joint limits
# self._ik_solver.set_joint_limits([0.0]* self._ik_solver.number_of_joints, upper_bound)
self._pos_bound = pos_bound
self._ori_bound = ori_bound
def check_setting(self):
# check the info of model
print('ik_solver.base_link : ', self._ik_solver.base_link)
print('ik_solver.tip_link : ', self._ik_solver.tip_link)
print('ik_solver.joint_names: ', self._ik_solver.joint_names)
print('lower_bound : ', self._lower_bound)
print('upper_bound : ', self._upper_bound)
def calculate(self, pos, ori):
#calculate IK
result = self._ik_solver.get_ik(
self._seed_state,
pos[0],
pos[1],
pos[2],
ori[0],
ori[1],
ori[2],
ori[3],
self._pos_bound[0],
self._pos_bound[1],
self._pos_bound[2], # X, Y, Z bounds
self._ori_bound[0],
self._ori_bound[1],
self._ori_bound[2]) # Rotation X, Y, Z bounds
return result
class RobotKinematics:
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 calculate_inverse_kinematics(self,
joint_state,
x,
y,
z,
rx=0,
ry=0,
rz=0):
return self.ik_solver.get_ik(joint_state, x, y, z, rx, ry, rz, 1.0,
1e-8, 1e-8, 1e-8)
def calculate_inverse_kinematics_rl(self):
# Deep Deterministic Policy gradients
# https://blog.floydhub.com/robotic-arm-control-deep-reinforcement-learning/
pass
def calculate_direct_kinematic(self, joint_state):
forward_kin_matrix = self.kdl_kin.forward(joint_state)
x = forward_kin_matrix[0, -1]
y = forward_kin_matrix[1, -1]
z = forward_kin_matrix[2, -1]
return {"x": x, "y": y, "z": z}
def get_joint_names(self):
# return self.ik_solver.joint_names
return self.urdf_tree.joint_map.keys()
def get_link_names(self):
# return self.ik_solver.link_names
return self.urdf_tree.link_map.keys()
def move_to_pose(limb, pose):
pos, rot = pose
#trac_ik kinematics intializing
ik_solver = IK("base", "stp_021808TP00080_tip")
current_joint_angles = limb.joint_angles()
sorted_angles = sorted(current_joint_angles.items(),
key=lambda kv: (kv[0], kv[1]))
seed_state = [0] * ik_solver.number_of_joints
for i in range(len(sorted_angles)):
seed_state[i] = sorted_angles[i][1]
kin = ik_solver.get_ik(seed_state, pos[0], pos[1], pos[2], rot[0], rot[1],
rot[2], rot[3])
if kin == None:
return None
go_to_pose = dict(zip(limb.joint_names(), kin))
limb.move_to_joint_positions(go_to_pose)
class PathPlanner():
"""
This class should contain all path planning functionality.
The goal is that no ROS should be needed to interface with this node
All ROS should be self-contained here
"""
def __init__(self):
"""
Shouldn't need inputs. Stores:
2 move groups: arm and hand - moveit commander interface
planning scene interface and publisher
robot commander
IK solver (using trac_ik)
FK solver (moveit service)
"""
rospy.loginfo("To stop project CTRL + C")
rospy.on_shutdown(self.shutdown)
# Initialize moveit_commander
# I honestly don't think it actually uses the sys.argv argument,
# but I need to do a little more digging first. It parses the input
# for arguments containing "__name:=" which it passes to another initializer
# I need to dig further to see what the name parameter actually changes
moveit_commander.roscpp_initialize(sys.argv)
# Instatiate the move group
self.group = moveit_commander.MoveGroupCommander('arm')
self.group.set_planning_time(5)
self.group.set_workspace([-3, -3, -3, 3, 3, 3])
# This publishes trajectories to RVIZ for visualization
# Need to figure out how to get rid of the visualization as well
# Also how to use this to publish individual poses
self.display_planned_path_publisher = rospy.Publisher(
'arm/display_planned_path', DisplayTrajectory, queue_size=10)
# This publishes updates to the planning scene
self.planning_scene_publisher = rospy.Publisher('/collision_object',
CollisionObject,
queue_size=10)
# Planning scene
self.scene = moveit_commander.PlanningSceneInterface()
# RobotCommander
self.robot = moveit_commander.RobotCommander()
# IK Solver with trac_ik
# NOTE: Get this from the MoveGroup so it's adaptable to other robots
self.ik_solver = IK('root', 'm1n6s300_end_effector')
self.ik_default_seed = [0.0] * self.ik_solver.number_of_joints
# FK Solver
rospy.wait_for_service('/compute_fk')
self.fk_solver = rospy.ServiceProxy('/compute_fk', GetPositionFK)
rospy.sleep(2)
# Rate is 10 Hz. Is this a good rate?
rate = rospy.Rate(10)
def shutdown(self):
"""
Things to do when the node is shut down
(currently does nothing besides printing a message,
should it do anything?)
"""
rospy.loginfo("Stopping project")
rospy.sleep(1)
def plan_to_config(self, end_state):
"""
Plans to a configuration using MoveIt.
Does not deal with finger positions (these stay static)
-------
end_state: list of joint angles, discluding fingers (length 6)
-------
returns: moveit path object (array of JointStates or RobotStates I believe)
"""
joint_state = JointState()
joint_state.header.stamp = rospy.Time.now()
# the robot has a dumb base_link joint that we don't want
joint_state.name = self.group.get_joints()[1:-1]
print joint_state.name
joint_state.position = end_state
self.group.set_start_state_to_current_state()
try:
self.group.set_joint_value_target(joint_state)
except moveit_commander.MoveItCommanderException:
pass
# Plan the path
plan = self.group.plan()
return plan
def plan_to_pose(self, end_pose, seed=None):
"""
Plans to a pose by using inverse kinematics.
-------
end_pose: rigid_transform object from autolab? (or should I write my own? Or allow different inputs?)
seed: an optional input to the IK solver.
It's a configuration (len 6 array) from which the solver will start. (trac_ik uses an augmented newton's method)
If it's None, it'll use the current pose as the seed
-------
returns: moveit path object
"""
raiseNotDefined()
def move_home(self):
"""
Plans from the current state to the home position
-------
returns: moveit path object
"""
self.group.set_start_state_to_current_state()
self.group.set_named_target('Home')
self.group.set_workspace([-3, -3, -3, 3, 3, 3])
plan = self.group.plan()
return plan
def execute_path(self, path, wait_bool=True):
"""
Executes an input path in the real world / gazebo / rviz
------
path: a moveit path object
wait_bool: whether the node pauses until the robot reaches its destination
"""
self.group.execute(path, wait=wait_bool)
def get_ik(self, pose, seed=None, xyz_bounds=None, rpy_bounds=None):
"""
Uses trac_ik to get a joint configuration from an input pose
Does not include collision checking
------
pose: rigid transform from autolab? See plan_to_pose
seed: list of size 6. start value for the IK. See plan_to_pose. If None, will use current pose
xyz_bounds: list of size 3. xyz bounds of the end effector in meters. This allows an approximate ik solution. Default is None
rpy_bounds: a list of size 3. roll pitch yaw bounds of the end effector in radians. This allows an approximate ik solution. Default is None
------
returns: joint states, a list of size 6 (or None for failure)
"""
if seed_state is None:
seed_state = self.ik_default_seed
if pose.header.frame_id != self.ik_solver.base_link:
raise ValueError("Frame ID is not the root link")
position = pose.pose.position
orientation = pose.pose.orientation
print seed_state, position.x, position.y, \
position.z, orientation.x, orientation.y, \
orientation.z, orientation.w \
if xyz_bounds is None or rpy_bounds is None:
state = self.ik_solver.get_ik(seed_state, position.x, position.y,
position.z, orientation.x,
orientation.y, orientation.z,
orientation.w)
else:
state = self.ik_solver.get_ik(
seed_state, position.x, position.y, position.z, orientation.x,
orientation.y, orientation.z, orientation.w, xyz_bounds[0],
xyz_bounds[1], xyz_bounds[2], rpy_bounds[0], rpy_bounds[1],
rpy_bounds[2])
return state
def get_fk(self, joints):
"""
Uses the MoveIt service to get the end effector pose from joints
Note that this is the pose of the beginning of the hand, not the hand itself
------
joints: a list of size 6. Joint states for the calculation
------
returns: a rigid transform from autolab? See plan_to_pose
"""
header = Header()
header.frame_id = self.ik_solver.base_link
robot_state = RobotState()
robot_state.joint_state.name = self.ik_solver.joint_names
robot_state.joint_state.position = joints
links = [self.ik_solver.tip_link]
return self.fk_solver(header, links, robot_state).pose_stamped[0]
def collision_free(self, joints):
"""
Checks if the configuration collides with anything in the planning scene
------
joints: a list of size 6. Configuration for the collision check
------
returns: boolean True if it's ok, False if it collides
"""
raiseNotDefined()
def add_stl_to_scene(self, id, fileName, pose, scale):
"""
Adds an STL object to the scene
------
id: object id (string)
fileName: location of the STL file
pose: autolab rigid_transform?
scale: len 3 list defining scale in x,y,z
"""
raiseNotDefined()
def add_box_to_scene(self, id, pose, size):
"""
Adds a box to the scene
------
id: object id (string)
pose: see above
size: len 3 list for xyz dimensions
"""
raiseNotDefined()
def populate_scene():
"""
Not sure how this will be implemented yet, but will basically
use the PathPlanner planning scene functionality to add
the scene items needed for our test/demo
"""
def remove_object_from_scene(self, id):
"""
removes object from the scene
------
id: object id
"""
raiseNotDefined()
def attach_object_to_robot(self, id):
"""
attaches object to the robot
------
id: object id
"""
raiseNotDefined()
def detach_object_from_robot(self, id):
"""
detaches object from robot
------
id: object id
"""
raiseNotDefined()
def visualize_plan(self, plan): # Untested
"""
Visualize the plan in RViz
"""
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = plan.points[0]
display_trajectory.trajectory.extend(plan.points)
self.display_planned_path_publisher.publish(display_trajectory)
def visualize_state(self, joint_state):
"""
Visualize a joint state in RViz
"""
raiseNotDefined()
def iterate_ik(self,
pose,
state_list=None,
xyz_bounds=None,
rpy_bounds=None,
iteration_limit=10):
"""
Iterates IK until a non-colliding solution is found, or it hits the iteration limit
------
pose: see get_ik
state_list: list of length 6 lists, each of which is an initial condition to be tested.
After the list is exhausted, the system will use pseudorandom start conditions
until the iteration count runs out (hopefully it'll pick random states intelligently)
xyz_bounds: see get_ik
rpy_bounds: see get_ik
iteration_limit: max number of initial conditions the algorithm will try (including the list)
Default is 10.
------
returns: joint states, a list of size 6 (or None for failure)
"""
raiseNotDefined()
def grasp_plan(self, pregrasp, grasp):
"""
Plans to a pregrasp pose, then plans to the grasp pose
Does not open/close fingers
This should be the main function used, and will use many of the other functions
It will (hopefully) select intelligent IK solutions
------
pregrasp: An autolab rigid_transform? see plan_to_pose
grasp: same as above
------
returns: either two plans concatenated or just executes it
"""
raiseNotDefined()
class CartesianPoseMoveitPlanner:
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_goal_pose_planner_node')
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander('kuka_arm')
#Reference link: https://answers.ros.org/question/255792/change-planner-in-moveit-python/
#self.group.set_planner_id('RRTstarkConfigDefault')
self.group.set_planner_id('RRTConnectkConfigDefault')
self.group.set_planning_time(10)
self.group.set_num_planning_attempts(3)
#self.group.set_planning_time(15)
#self.group.set_num_planning_attempts(1)
self.zero_joint_states = np.zeros(7)
self.left_home_pose = False
if self.left_home_pose:
self.home_joint_states = np.array([
0.0001086467455024831, 0.17398914694786072,
-0.00015721925592515618, -1.0467143058776855,
0.0006054198020137846, -0.00030679398332722485,
3.3859387258416973e-06
])
else:
self.home_joint_states = np.array([
0.0001086467455024831, -0.17398914694786072,
0.00015721925592515618, 1.0467143058776855,
0.0006054198020137846, -0.00030679398332722485,
3.3859387258416973e-06
])
self.ik_solver = IK('world', 'allegro_mount')
self.seed_state = [0.0] * self.ik_solver.number_of_joints
def go_home(self):
print 'go home'
self.group.clear_pose_targets()
self.group.set_joint_value_target(self.home_joint_states)
plan_home = self.group.plan()
return plan_home
def go_zero(self):
print 'go zero'
self.group.clear_pose_targets()
self.group.set_joint_value_target(self.zero_joint_states)
plan_home = self.group.plan()
return plan_home
def go_goal(self, pose):
print 'go goal'
self.group.clear_pose_targets()
self.group.set_pose_target(pose)
plan_goal = self.group.plan()
return plan_goal
def go_goal_trac_ik(self, pose):
print 'go goal'
self.group.clear_pose_targets()
ik_js = self.ik_solver.get_ik(self.seed_state, pose.position.x,
pose.position.y, pose.position.z,
pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
if ik_js is None:
rospy.logerr('No IK solution for motion planning!')
return None
self.group.set_joint_value_target(np.array(ik_js))
plan_goal = self.group.plan()
return plan_goal
def handle_pose_goal_planner(self, req):
plan = None
if req.go_home:
plan = self.go_home()
elif req.go_zero:
plan = self.go_zero()
else:
# plan = self.go_goal(req.palm_goal_pose_world)
plan = self.go_goal_trac_ik(req.palm_goal_pose_world)
#print plan
response = PalmGoalPoseWorldResponse()
response.success = False
if plan is None:
return response
if len(plan.joint_trajectory.points) > 0:
response.success = True
response.plan_traj = plan.joint_trajectory
return response
def create_moveit_planner_server(self):
rospy.Service('moveit_cartesian_pose_planner', PalmGoalPoseWorld,
self.handle_pose_goal_planner)
rospy.loginfo('Service moveit_cartesian_pose_planner:')
rospy.loginfo('Reference frame: %s' % self.group.get_planning_frame())
rospy.loginfo('End-effector frame: %s' %
self.group.get_end_effector_link())
rospy.loginfo('Robot Groups: %s' % self.robot.get_group_names())
rospy.loginfo('Ready to start to plan for given palm goal poses.')
def handle_arm_movement(self, req):
self.group.go(wait=True)
response = MoveArmResponse()
response.success = True
return response
def create_arm_movement_server(self):
rospy.Service('arm_movement', MoveArm, self.handle_arm_movement)
rospy.loginfo('Service moveit_cartesian_pose_planner:')
rospy.loginfo('Ready to start to execute movement plan on robot arm.')
from trac_ik_python.trac_ik import IK
ik_solver = IK(base_link="base_link",
tip_link="tool0",
timeout=0.005,
epsilon=1e-5,
solve_type="Speed",
urdf_string=None)
seed_state = [0.0] * ik_solver.number_of_joints
qa = [-0.131, 0.337, 0.263, -1., 0., 0., -0.019]
print(ik_solver.get_ik(seed_state, -0.131, 0.337, 0.263, -1., 0., 0., -0.019))
seed_state = [0.3584, -0.80591, 2.14269, -3.55912, -1.82971, 0.43981]
print(
ik_solver.get_ik(seed_state, -0.131, 0.269, 0.006, -0.383, 0., 0., -0.924))
class PR2Teleop(object):
def __init__(self):
self.ik_right = IK("torso_lift_link",
#"r_wrist_roll_link")
"r_gripper_tool_frame")
self.ik_left = IK("torso_lift_link",
"l_wrist_roll_link")
self.left_command = rospy.Publisher('/l_arm_controller/command',
JointTrajectory,
queue_size=1)
self.right_command = rospy.Publisher('/r_arm_controller/command',
JointTrajectory,
queue_size=1)
self.last_left_pose = None
self.left_pose = rospy.Subscriber('/left_controller_as_posestamped',
PoseStamped,
self.left_cb, queue_size=1)
self.last_right_pose = None
self.right_pose = rospy.Subscriber('/right_controller_as_posestamped',
PoseStamped,
self.right_cb, queue_size=1)
rospy.sleep(2.0)
def left_cb(self, msg):
self.last_left_pose = msg
def right_cb(self, msg):
self.last_right_pose = msg
def send_right_arm_goal(self, positions):
jt = JointTrajectory()
jt.header.stamp = rospy.Time.now()
jt.joint_names = ["r_shoulder_pan_joint", "r_shoulder_lift_joint", "r_upper_arm_roll_joint",
"r_elbow_flex_joint", "r_forearm_roll_joint", "r_wrist_flex_joint", "r_wrist_roll_joint"]
jtp = JointTrajectoryPoint()
jtp.positions = list(positions)
jtp.velocities = [0.0] * len(positions)
jtp.time_from_start = rospy.Time(0.4)
jt.points.append(jtp)
# print("Goal: ")
# print(jt)
self.right_command.publish(jt)
def send_left_arm_goal(self, positions):
jt = JointTrajectory()
jt.header.stamp = rospy.Time.now()
jt.joint_names = ["l_shoulder_pan_joint", "l_shoulder_lift_joint", "l_upper_arm_roll_joint",
"l_elbow_flex_joint", "l_forearm_roll_joint", "l_wrist_flex_joint", "l_wrist_roll_joint"]
jtp = JointTrajectoryPoint()
jtp.positions = list(positions)
jtp.velocities = [0.0] * len(positions)
jtp.time_from_start = rospy.Time(0.1)
jt.points.append(jtp)
self.left_command.publish(jt)
def run_with_ik(self):
qinit = [0., 0., 0., 0., 0., 0., 0.]
x = y = z = 0.0
rx = ry = rz = 0.0
rw = 1.0
bx = by = bz = 0.02
brx = bry = brz = 0.5
r = rospy.Rate(4)
while not rospy.is_shutdown():
ps = self.last_right_pose
if ps is None:
r.sleep()
print("No last right pose...")
continue
x = self.last_right_pose.pose.position.x
y = self.last_right_pose.pose.position.y
z = self.last_right_pose.pose.position.z
rx = self.last_right_pose.pose.orientation.x
ry = self.last_right_pose.pose.orientation.y
rz = self.last_right_pose.pose.orientation.z
rw = self.last_right_pose.pose.orientation.w
# rospy.loginfo("Got pose: " + str(ps))
sol = None
retries = 0
while not sol and retries < 10:
sol = self.ik_right.get_ik(qinit,
x, y, z,
rx, ry, rz, rw,
bx, by, bz,
brx, bry, brz)
retries += 1
if sol:
print "Solution found: (" + str(retries) + " retries)"
print sol
self.send_right_arm_goal(sol)
qinit = sol
else:
print "NO SOLUTION FOUND :("
r.sleep()
class Controller:
def __init__(self):
# initialize publishers, subscribers, tflisteners
self.arm_pub = rospy.Publisher('/goal_dynamixel_position',
JointState,
queue_size=1)
self.pan_pub = rospy.Publisher('/pan/command', Float64, queue_size=1)
self.tilt_pub = rospy.Publisher('/tilt/command', Float64, queue_size=1)
self.gripper_open_pub = rospy.Publisher('/gripper/open',
Empty,
queue_size=1)
self.gripper_close_pub = rospy.Publisher('/gripper/close',
Empty,
queue_size=1)
rospy.Subscriber('/joint_states', JointState, self.get_joint_state)
self.tf_listener = tf.TransformListener()
self.history = {'timestamp': deque(), 'joint_feedback': deque()}
# global variables
self.current_joint_state = None
self.current_gripper_state = None
self.current_target_state = None
# global frames
self.GRIPPER_LINK = "gripper_link"
self.BOTTOM_PLATE_LINK = "bottom_plate"
# other classes
self.ik_solver = IK(self.BOTTOM_PLATE_LINK, self.GRIPPER_LINK)
# predefined variables
self.HOME_POS_MANIPULATOR_00 = [0.0, 0.0, 0.0, 0.0, 0.0]
self.HOME_POS_MANIPULATOR_01 = [
0.004601942375302315, -0.4218447208404541, 1.6260197162628174,
-0.1426602154970169, 0.010737866163253784
]
self.HOME_POS_MANIPULATOR_02 = [0.0, 0.0, 1.22, -1.57, 1.57]
self.HOME_POS_CAMERA_01 = [0.0, 0.698]
self.HOME_POS_CAMERA_02 = [-0.452, -0.452]
self.IK_POSITION_TOLERANCE = 0.01
self.IK_ORIENTATION_TOLERANCE = np.pi / 3
self.MIN_CLOSING_GAP = 0.002
self.MOVEABLE_JOINTS = [0, 1, 4]
self.CONVERGENCE_CRITERION = 0.1
self.CONVERGENCE_CRITERION_COUNT = 10
def set_camera_angles(self, angles):
pan_msg = Float64()
pan_msg.data = float(angles[0])
rospy.loginfo('Going to camera pan: {} rad'.format(angles[0]))
self.pan_pub.publish(pan_msg)
tilt_msg = Float64()
tilt_msg.data = float(angles[1])
rospy.loginfo('Going to camera tilt: {} rad'.format(angles[1]))
self.tilt_pub.publish(tilt_msg)
def set_arm_joint_angles(self, joint_target):
joint_state = JointState()
joint_state.position = tuple(joint_target)
self.arm_pub.publish(joint_state)
convergence_count = 0
while (convergence_count < self.CONVERGENCE_CRITERION_COUNT):
if (np.sum(
np.abs(
np.asarray(self.current_joint_state) -
np.asarray(joint_target))) <
self.CONVERGENCE_CRITERION):
convergence_count = convergence_count + 1
else:
convergence_count = 0
rospy.loginfo("CONVERGED!")
def get_joint_state(self, data):
# TODO: Change this when required
# Add timestamp
self.history['timestamp'].append(time.time())
if (len(self.history['timestamp']) > 2):
self.history['timestamp'].popleft()
# Add Joint Feedback
joint_angles = np.array(data.position)[self.MOVEABLE_JOINTS]
self.history['joint_feedback'].append(joint_angles)
if (len(self.history['joint_feedback']) > 2):
self.history['joint_feedback'].popleft()
self.current_joint_state = data.position[0:5]
self.current_gripper_state = data.position[7:9]
def open_gripper(self):
empty_msg = Empty()
rospy.loginfo('Opening gripper')
self.gripper_open_pub.publish(empty_msg)
rospy.sleep(4)
def close_gripper(self):
empty_msg = Empty()
rospy.loginfo('Closing gripper')
self.gripper_close_pub.publish(empty_msg)
rospy.sleep(4)
# finds the inverse kinematics solution for the target pose
def compute_ik(self, target_pose):
"""
Parameters
----------
ik_solver: trac_ik_python.trac_ik Ik object
target_pose: type geometry_msgs/Pose
current_joint: list with length the number of joints (i.e. 5)
Returns
----------
IK solution (a list of joint angles for target_pose)
if found, None otherwise
"""
result = self.ik_solver.get_ik(
self.current_joint_state, target_pose.position.x,
target_pose.position.y, target_pose.position.z,
target_pose.orientation.x, target_pose.orientation.y,
target_pose.orientation.z, target_pose.orientation.w,
self.IK_POSITION_TOLERANCE, self.IK_POSITION_TOLERANCE,
self.IK_POSITION_TOLERANCE, self.IK_ORIENTATION_TOLERANCE,
self.IK_ORIENTATION_TOLERANCE, self.IK_ORIENTATION_TOLERANCE)
if result:
rospy.loginfo('IK solution found')
else:
rospy.logerr('No IK solution found')
return result
# Goes to the position given by FRAME and grabs the object from the top
def go_to_grasp_pose(self, FRAME):
try:
self.tf_listener.waitForTransform(self.BOTTOM_PLATE_LINK, FRAME,
rospy.Time.now(),
rospy.Duration(5.0))
P, Q = self.tf_listener.lookupTransform(self.BOTTOM_PLATE_LINK,
FRAME, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
return False
O = tf.transformations.euler_from_quaternion(Q)
Q = tf.transformations.quaternion_from_euler(0, np.pi / 2, O[2])
poses = [
Pose(Point(P[0], P[1], P[2] + 0.15),
Quaternion(Q[0], Q[1], Q[2], Q[3])),
Pose(Point(P[0], P[1], P[2] + 0.10),
Quaternion(Q[0], Q[1], Q[2], Q[3]))
]
print(poses)
target_joint = None
self.open_gripper()
for pose in poses:
if self.current_joint_state:
target_joint = self.compute_ik(pose)
else:
print("Joint State Subscriber not working")
return False
if target_joint:
self.set_arm_joint_angles(target_joint)
self.current_target_state = np.array(target_joint)[
self.MOVEABLE_JOINTS]
else:
return False
return True
# checks if the object was grasped or not
def check_grasp(self):
if self.current_gripper_state:
if(np.abs(self.current_gripper_state[0]) < self.MIN_CLOSING_GAP \
and np.abs(self.current_gripper_state[1] < self.MIN_CLOSING_GAP)):
print("Coundn't grasp the object")
return False
else:
return True
else:
print("Joint State Subscriber not working")
return False
# Goes to the position given by FRAME and grabs the object from the top
def go_to_handover_pose(self, pose):
print(pose)
try:
self.tf_listener.waitForTransform(self.BOTTOM_PLATE_LINK,
pose.header.frame_id,
rospy.Time.now(),
rospy.Duration(5.0))
P, Q = self.tf_listener.lookupTransform(self.BOTTOM_PLATE_LINK,
pose.header.frame_id,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
return False
O = tf.transformations.euler_from_quaternion(Q)
Q = tf.transformations.quaternion_from_euler(0, -np.pi / 3, O[2])
poses = Pose(Point(P[0], P[1], P[2] - 0.45),
Quaternion(Q[0], Q[1], Q[2], Q[3]))
target_joint = None
while target_joint is None:
if self.current_joint_state:
target_joint = self.compute_ik(poses)
else:
print("Joint State Subscriber not working")
return False
if target_joint:
print(target_joint)
self.set_arm_joint_angles(target_joint)
self.current_target_state = np.array(target_joint)[
self.MOVEABLE_JOINTS]
else:
rospy.logwarn("No Solution was found. Current Tolerance " +
str(self.IK_POSITION_TOLERANCE))
self.IK_POSITION_TOLERANCE += 0.05
print(target_joint)
return True
# Goes to the position given by FRAME and grabs the object from the top
def go_to_data_collection_pose(self, pose):
# print(pose)
# poses = Pose(Point(pose[0], pose[1], pose[2]), Quaternion(pose[3], pose[4], pose[5], pose[6]))
target_joint = None
while target_joint is None:
if self.current_joint_state:
target_joint = self.compute_ik(pose)
else:
print("Joint State Subscriber not working")
return False
if target_joint:
print(target_joint)
self.set_arm_joint_angles(target_joint)
self.current_target_state = np.array(target_joint)[
self.MOVEABLE_JOINTS]
else:
rospy.logwarn("No Solution was found. Current Tolerance " +
str(self.IK_POSITION_TOLERANCE))
self.IK_POSITION_TOLERANCE += 0.05
print(target_joint)
return True
class PR2Teleop(object):
def __init__(self):
urdf = rospy.get_param('/robot_description')
self.ik_right = IK("base_link", "ee_link", 0.005, 1e-5, "Distance",
urdf)
#self.ik_left = IK("torso_lift_link",
# "l_wrist_roll_link")
#self.left_command = rospy.Publisher('/l_arm_controller/command',
# JointTrajectory,
# queue_size=1)
self.right_command = rospy.Publisher('/arm_controller/command',
JointTrajectory,
queue_size=1)
#self.last_left_pose = None
#self.left_pose = rospy.Subscriber('/left_controller_as_posestamped',
#PoseStamped,
#self.left_cb, queue_size=1)
self.last_right_pose = None
self.right_pose = rospy.Subscriber(
'/free_positioning/gripper_marker_pose',
PoseStamped,
self.right_cb,
queue_size=1)
rospy.sleep(2.0)
def left_cb(self, msg):
self.last_left_pose = msg
def right_cb(self, msg):
self.last_right_pose = msg
def send_right_arm_goal(self, positions):
jt = JointTrajectory()
jt.header.stamp = rospy.Time.now()
jt.joint_names = [
"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
]
jtp = JointTrajectoryPoint()
jtp.positions = list(positions)
jtp.velocities = [0.0] * len(positions)
jtp.time_from_start = rospy.Time(0.08) #default 0.4
jt.points.append(jtp)
# print("Goal: ")
#print(jt)
self.right_command.publish(jt)
'''
def send_left_arm_goal(self, positions):
jt = JointTrajectory()
jt.header.stamp = rospy.Time.now()
jt.joint_names = ["l_shoulder_pan_joint", "l_shoulder_lift_joint", "l_upper_arm_roll_joint",
"l_elbow_flex_joint", "l_forearm_roll_joint", "l_wrist_flex_joint", "l_wrist_roll_joint"]
jtp = JointTrajectoryPoint()
jtp.positions = list(positions)
jtp.velocities = [0.0] * len(positions)
jtp.time_from_start = rospy.Time(0.1)
jt.points.append(jtp)
self.left_command.publish(jt)
'''
def run_with_ik(self):
qinit = [0., 0., 0., 0., 0., 0.]
x = y = z = 0.0
rx = ry = rz = 0.0
rw = 1.0
bx = by = bz = 0.0 #default 0.02
brx = bry = brz = 0.0 #default 0.5
r = rospy.Rate(125) #default 4
while not rospy.is_shutdown():
ps = self.last_right_pose
if ps is None:
r.sleep()
print("No last right pose...")
continue
x = self.last_right_pose.pose.position.x
y = self.last_right_pose.pose.position.y
z = self.last_right_pose.pose.position.z
rx = self.last_right_pose.pose.orientation.x
ry = self.last_right_pose.pose.orientation.y
rz = self.last_right_pose.pose.orientation.z
rw = self.last_right_pose.pose.orientation.w
# rospy.loginfo("Got pose: " + str(ps))
sol = None
retries = 0
start = time.clock()
while not sol and retries < 10:
sol = self.ik_right.get_ik(qinit, x, y, z, rx, ry, rz, rw, bx,
by, bz, brx, bry, brz)
retries += 1
end = time.clock()
print "Execution time:" + str(1000 * (end - start)) + "ms"
if sol:
print "Solution found: (" + str(retries) + " retries)"
#print sol
self.send_right_arm_goal(sol)
qinit = sol
else:
print "NO SOLUTION FOUND :("
r.sleep()
ind += 1
#first seed is zero-vector for trajectory evaluation
if use_last_solution_as_seed:
seed_state = np.zeros_like(seed_configurations[0,:])
#solve for validation data
joints_num_val = np.zeros((X_val.shape[0], ik_solver.number_of_joints))
sample_times = []
for i in range(joints_num_val.shape[0]):
if not use_last_solution_as_seed:
seed_state = seed_configurations[i,:]
begin = timer()
joints_i = ik_solver.get_ik(seed_state, #initial configuration
X_val[i,0], X_val[i,1], 0.0, #desired position (x, y, z)
0.0, 0.0, math.sin(X_val[i,2]/2), math.cos(X_val[i,2]/2), #desired orientation (quaternions: xq, yq, zq, wq)
pos_error, pos_error, pos_error, #allowed position error (x, y, z)
ori_error, ori_error, ori_error) #allowed orientation error (rotation over axis x, y, z)
end = timer()
sample_times.append(end - begin)
joints_num_val[i,:] = joints_i
if use_last_solution_as_seed and not np.any(np.isnan(joints_num_val[i,:])):
seed_state = joints_num_val[i,:] #for trajectory evaluation: seed is always the current configuration (last solution)
print("Sum TRAC-IK: "+str(np.sum(sample_times))+" seconds")
print("Mean TRAC-IK: "+str(np.mean(sample_times))+" seconds")
print("Std TRAC-IK: "+str(np.std(sample_times))+" seconds")
np.save("data/joints_num_val.npy", joints_num_val)
class CtrlEndEffPos(object):
"""Control end effector position
Attributes
hebi_group_name (str):
hebi_families (list of str): HEBI actuator families [base,..., tip]
hebi_names (list of str): HEBI actuator names [base,..., tip]
from_master_topic (str):
to_master_topic (str):
base_link_name (str): arm base link name in urdf
end_link_name (str): end base link name in urdf
ik_timeout (float):
ik_epsilon (float):
ik_solve_type (str): https://bitbucket.org/traclabs/trac_ik/src/HEAD/trac_ik_lib/
ik_position_bounds (list of float): [x_pos_bound, y_pos_bound, z_pos_bound]
"""
def __init__(self, hebi_group_name, hebi_families, hebi_names,
from_master_topic, to_master_topic,
base_link_name, end_link_name,
ik_timeout, ik_epsilon, ik_solve_type, ik_position_bounds):
self.hebi_mapping = []
self.current_jt_pos = {}
self.current_jt_vel = {}
self.current_jt_eff = {}
self._last_valid_jt_ang = []
self._msg_from_master = None
## ROS node setup
rospy.init_node('end_eff_pos_ctrl_node') # Should be renamed in .launch
self._rate = rospy.Rate(200) #NOTE: Could set via arg
## HEBI setup - NOTE: hebiros_node must be running
self.hebi_group_name = hebi_group_name
rospy.loginfo("HEBI Group name: "+ str(hebi_group_name))
self.hebi_families = hebi_families
rospy.loginfo("HEBI families: "+ str(hebi_families))
self.hebi_names = hebi_names
rospy.loginfo("HEBI names: "+ str(hebi_names))
self.hebi_mapping = [family+'/'+name for family,name in zip(hebi_families,hebi_names)]
rospy.loginfo("self.hebi_mapping: "+ str(self.hebi_mapping))
# Create a service client to create HEBI group
set_hebi_group = rospy.ServiceProxy('/hebiros/add_group_from_names', AddGroupFromNamesSrv)
# Topic to receive feedback from HEBI group
hebi_group_feedback_topic = "/hebiros/"+hebi_group_name+"/feedback/joint_state"
# Topic to send commands to HEBI group
hebi_group_command_topic = "/hebiros/"+hebi_group_name+"/command/joint_state"
# Call the /hebiros/add_group_from_names service to create a group
rospy.wait_for_service('/hebiros/add_group_from_names')
set_hebi_group(hebi_group_name,hebi_names,hebi_families)
## ROS pub/sub setup
self._master_sub = rospy.Subscriber(from_master_topic, Pose, self._master_cb)
self._master_pub = rospy.Publisher(to_master_topic, String, queue_size=1)
self._arm_sub = rospy.Subscriber(hebi_group_feedback_topic, JointState, self._arm_cb)
self._arm_pub = rospy.Publisher(hebi_group_command_topic, JointState, queue_size=1)
## Simulation
self._base_pub = rospy.Publisher("/to_x5_base", Float32, queue_size=1)
self._shoulder_pub = rospy.Publisher("/to_x5_shoulder", Float32, queue_size=1)
self._elbow_pub = rospy.Publisher("/to_x5_elbow", Float32, queue_size=1)
## trac_ik setup
# Get urdf from ROS parameter server
urdf_str = ""
urdf_loaded = False
while not rospy.is_shutdown() and not urdf_loaded:
if rospy.has_param('/robot_description'): #NOTE: Could set via arg
urdf_str = rospy.get_param('/robot_description')
urdf_loaded = True
rospy.loginfo("Pulled /robot_description from parameter server.")
else:
rospy.sleep(0.2) # sleep for 0.2s of ROS time
self._ik_solver = IK(base_link_name,
end_link_name,
urdf_string=urdf_str,
timeout=ik_timeout,
epsilon=ik_epsilon,
solve_type=ik_solve_type)
rospy.loginfo(" self.ik_solver.base_link: %s", self._ik_solver.base_link)
rospy.loginfo(" self.ik_solver.tip_link: %s", self._ik_solver.tip_link)
rospy.loginfo(" self.ik_solver.link_names: %s", self._ik_solver.link_names)
rospy.loginfo(" self.ik_solver.joint_names: %s", self._ik_solver.joint_names)
lb, ub = self._ik_solver.get_joint_limits()
rospy.loginfo(" self.ik_solver.get_joint_limits()")
rospy.loginfo(" lower_bounds: %s", lb)
rospy.loginfo(" upper_bounds: %s", ub)
self._xyz_position_bounds = ik_position_bounds
rospy.loginfo(" self._xyz_position_bounds: %s", self._xyz_position_bounds)
self._orientation_bounds = [31416.0, 31416.0, 31416.0] #NOTE: This implements position-only IK
rospy.loginfo(" self._orientation_bounds: %s", self._orientation_bounds)
## pykdl_utils setup
robot_urdf = URDF.from_xml_string(urdf_str)
self._kdl_kin = KDLKinematics(robot_urdf, base_link_name, end_link_name)
# Wait for connections to be setup
while not rospy.is_shutdown() and len(self.current_jt_pos) < len(self.hebi_mapping):
rospy.sleep(0.2) # sleep for 0.2s of ROS time
jt_angles = [self.current_jt_pos[motor] for motor in self.hebi_mapping]
pose = self._kdl_kin.forward(jt_angles)
rospy.loginfo("current pose: %s", pose)
self.current_eff_pos = [round(pose[0,3], 5), round(pose[1,3], 5), round(pose[2,3], 5)]
rospy.loginfo("self.current_eff_pos [x, y, z]: %s", self.current_eff_pos)
#TODO: Probably should include pose information too!!!
# Would need to convert homogeneous matrix to proper normalized quaternion.
def start(self):
## main loop
max_step = 0.01 # m - NOTE: Could set via message field
while not rospy.is_shutdown():
if self._msg_from_master is None:
if self._last_valid_jt_ang is not None:
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = self.hebi_mapping
msg.position = self._last_valid_jt_ang
msg.velocity = [] #TODO
msg.effort = [] #TODO: Gravity compensation
self._arm_pub.publish(msg)
else:
target_pos = [self._msg_from_master.position.x, self._msg_from_master.position.y, self._msg_from_master.position.z]
rospy.loginfo(" target_pos: %s", target_pos)
self._msg_from_master = None
squared_dif = (target_pos[0]-self.current_eff_pos[0])**2.0 \
+ (target_pos[1]-self.current_eff_pos[1])**2.0 \
+ (target_pos[2]-self.current_eff_pos[2])**2.0
if squared_dif < 1e-6:
rospy.loginfo(" Target end-effector position is same as current position!\n"
+ " Please try another pt...")
else:
straight_line_dist = m.sqrt(squared_dif)
rospy.loginfo(" straight_line_dist: %s [m]", straight_line_dist)
num_pts = int(m.ceil(straight_line_dist/max_step))
rospy.loginfo(" num_pts: %s", num_pts)
x_dif = target_pos[0]-self.current_eff_pos[0]
y_dif = target_pos[1]-self.current_eff_pos[1]
z_dif = target_pos[2]-self.current_eff_pos[2]
x_delta = x_dif / num_pts
y_delta = y_dif / num_pts
z_delta = z_dif / num_pts
xyz_pts=[]
for i in range(num_pts):
pt = (self.current_eff_pos[0] + (i+1)*x_delta,
self.current_eff_pos[1] + (i+1)*y_delta,
self.current_eff_pos[2] + (i+1)*z_delta)
xyz_pts.append(pt)
wxyz_pts = [(0,0,0,1) for pt in xyz_pts] #NOTE: Temporary - still only interested in position-only IK
result, joint_trajectory = self._generate_joint_trajectory(xyz_pts=xyz_pts, wxyz_pts=wxyz_pts)
if not result:
rospy.loginfo(" IK FAILED! Joint solution not found.\n"
+ " Please try another pt...")
self._master_pub.publish("failure")
else:
rospy.loginfo(" IK SUCCESS! Joint solution found.")
rospy.loginfo(" Executing joint_trajectory...")
avg_vel = 0.25 #NOTE: This is not a good way to do this. I need to use some kind of trajectory filter.
time_to_execute = straight_line_dist / avg_vel
self._execute_joint_trajectory(joint_trajectory, time_to_execute)
rospy.loginfo(" Finished executing joint_trajectory.")
self._master_pub.publish("success")
pose = self._kdl_kin.forward(self._last_valid_jt_ang)
self.current_eff_pos = [round(pose[0,3], 5), round(pose[1,3], 5), round(pose[2,3], 5)] # Would need to convert homogeneous matrix to proper normalized quaternion.
rospy.loginfo("self.current_eff_pos [x, y, z]: %s", self.current_eff_pos)
self._rate.sleep()
def _arm_cb(self, msg):
for name, pos, vel, eff in zip(msg.name, msg.position, msg.velocity, msg.effort):
if name not in self.hebi_mapping:
print("WARNING: arm_callback - unrecognized name!!!")
else:
self.current_jt_pos[name] = pos
self.current_jt_vel[name] = vel
self.current_jt_eff[name] = eff
def _master_cb(self, msg):
self._msg_from_master = msg
def _generate_joint_trajectory(self, xyz_pts, wxyz_pts):
seed_state = [self.current_jt_pos[motor] for motor in self.hebi_mapping]
joint_trajectory = []
for pos,rot in zip(xyz_pts,wxyz_pts):
rospy.loginfo(" seed_state: %s", seed_state)
target_jt_ang = self._ik_solver.get_ik(seed_state,
pos[0],pos[1],pos[2],
rot[0],rot[1],rot[2],rot[3],
self._xyz_position_bounds[0],
self._xyz_position_bounds[1],
self._xyz_position_bounds[2],
self._orientation_bounds[0],
self._orientation_bounds[1],
self._orientation_bounds[2])
rospy.loginfo(" target_jt_ang: %s", target_jt_ang)
if target_jt_ang is None:
return False, None
joint_trajectory.append(target_jt_ang)
seed_state = target_jt_ang # for next iteration
# Maintain current position while planning - TODO: better to replace with HEBI Trajectory Action?
if self._last_valid_jt_ang is not None:
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = self.hebi_mapping
msg.position = self._last_valid_jt_ang
msg.velocity = []
msg.effort = []
self._arm_pub.publish(msg)
return True, joint_trajectory
def _execute_joint_trajectory(self, joint_trajectory, time_to_execute):
num_pts = len(joint_trajectory)
time_start = rospy.Time.now().to_sec() # works with simulation time too
time_end = time_start + time_to_execute
time_incr = (time_end - time_start) / float(num_pts)
index = 0
done = False
while not rospy.is_shutdown() and not done:
time_now = rospy.Time.now().to_sec()
if time_now > time_end:
done = True
else:
if time_now > time_start + index*time_incr:
if index < num_pts-1:
index += 1
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = self.hebi_mapping
msg.position = joint_trajectory[index]
msg.velocity = [] #TODO
msg.effort = [] #TODO: Gravity compensation
self._arm_pub.publish(msg)
## Simulation
self._base_pub.publish(joint_trajectory[index][0])
self._shoulder_pub.publish(joint_trajectory[index][1])
self._elbow_pub.publish(joint_trajectory[index][2])
self._rate.sleep()
self._last_valid_jt_ang = joint_trajectory[index]
def main():
rospy.init_node('Joint_Control', anonymous='True')
robot_controller = RobotController()
rate = rospy.Rate(60)
with open('/home/kurt/Robotics/src/ar3/urdf/AR3_noGazebo.urdf', 'r') as fp:
urdf = fp.read()
Solver = IK("world", "flange", urdf_string=urdf)
print("IK solver uses link chain:")
print(Solver.link_names)
print("IK solver base frame:")
print(Solver.base_link)
print("IK solver tip link:")
print(Solver.tip_link)
print("IK solver for joints:")
print(Solver.joint_names)
# print("IK solver using joint limits:")
# lb, up = Solver.get_joint_limits()
# print("Lower bound: " + str(lb))
# print("Upper bound: " + str(up))
qinit = [0.0] * Solver.number_of_joints
# print(Solver.number_of_joints)
x = 0.0
y = -0.3
z = 0.35
# quat = quaternion_from_euler(0.0,3.14,0.0,'rxyz')
rx, ry, rz, rw = 1, 0, 0, .007
bx = by = bz = .03
brx = bry = brz = 2 * pi * .1
# sol = Solver.get_ik(qinit,x,y,z,rx,ry,rz,rw,bx,by,bz,brx,bry,brz)
sol = Solver.get_ik(qinit, x, y, z, rx, ry, rz, rw)
# sol = _ik_solver.CartToJnt(qinit,x,y,z,rx,ry,rz,rw)
if not sol:
print('No solution found!')
return
print("Solution found: ", sol)
# userIn = raw_input("Can the robot begin moving? Ensure the space is clear (y/n): ")
# assert userIn=='y','Robot wont begin moving.'
robot_controller.AR3Control.home = 0
robot_controller.AR3Control.run = 1
robot_controller.AR3Control.rest = 0
while not rospy.is_shutdown():
angs = list(sol)
if angs:
# angs.pop()
robot_controller.AR3Control.joint_angles = angs
robot_controller.send_joints()
class PathPlanner(object):
def __init__(self):
"""
Path Planner Class.
References:
dynamicreplanning.weebly.com,
moveit python interface tutorial,
trac_ik python tutorial
jaco_manipulation
"""
rospy.loginfo("To stop project CTRL + C")
rospy.on_shutdown(self.shutdown)
# Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
# Instatiate the move group
self.group = moveit_commander.MoveGroupCommander('arm')
self.group.set_planning_time(5)
self.group.set_workspace([-3, -3, -3, 3, 3, 3])
# This publishes trajectories to RVIZ for visualization
self.display_planned_path_publisher = rospy.Publisher(
'arm/display_planned_path', DisplayTrajectory, queue_size=10)
# This publishes updates to the planning scene
self.planning_scene_publisher = rospy.Publisher('/collision_object',
CollisionObject,
queue_size=10)
# Planning scene
self.scene = moveit_commander.PlanningSceneInterface()
# RobotCommander
self.robot = moveit_commander.RobotCommander()
# IK Solver with trac_ik
# NOTE: Get this from the MoveGroup so it's adaptable to other robots
self.ik_solver = IK('root', 'm1n6s300_end_effector')
self.ik_default_seed = [0.0] * self.ik_solver.number_of_joints
# FK Solver
rospy.wait_for_service('/compute_fk')
self.fk_solver = rospy.ServiceProxy('/compute_fk', GetPositionFK)
rospy.sleep(2)
rate = rospy.Rate(10)
def shutdown(self):
rospy.loginfo("Stopping project")
rospy.sleep(1)
def plan_to_config(self, end_state):
"""
Uses MoveIt to plan a path from the current state to end_state and returns it
end_state: list of 6 joint values
"""
joint_state = JointState()
joint_state.header.stamp = rospy.Time.now()
# the robot has a dumb base_link joint that we don't want
joint_state.name = self.group.get_joints()[1:-1]
print joint_state.name
joint_state.position = end_state
self.group.set_start_state_to_current_state()
try:
self.group.set_joint_value_target(joint_state)
except moveit_commander.MoveItCommanderException:
pass
# Plan the path
plan = self.group.plan()
return plan
def execute_path(self, path, wait_bool=True):
"""
Executes a provided path.
Note that the current position must be the same as the path's initial position.
This is currently not checked.
"""
self.group.execute(path, wait=wait_bool)
# def collision_free_move_pose(self, end_pose):
# """
# Uses MoveIt to plan a path from the current state to end effector pose end_pose
# end_pose: a PoseStamped object for the end effector
# """
# self.group.set_start_state_to_current_state()
# self.group.set_joint_value_target(end_pose)
# self.group.set_workspace([-3, -3, -3, 3, 3, 3])
# plan = self.group.plan()
# return plan
def move_home(self):
"""
Uses MoveIt to plan a path from the current state to the home position
"""
self.group.set_start_state_to_current_state()
self.group.set_named_target('Home')
self.group.set_workspace([-3, -3, -3, 3, 3, 3])
plan = self.group.plan()
return plan
def visualize_plan(self, plan):
"""
Visualize the plan in RViz
"""
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = plan.points[0]
display_trajectory.trajectory.extend(plan.points)
self.display_planned_path_publisher.publish(display_trajectory)
def make_pose(self, position, orientation, frame):
"""
Creates a PoseStamped message based on provided position and orientation
position: list of size 3
orientation: list of size 4 (quaternion) (wxyz)
frame: string (the reference frame to which the pose is defined)
returns pose: a PoseStamped object
"""
pose = PoseStamped()
pose.header.frame_id = frame
pose.pose.position.x = position[0]
pose.pose.position.y = position[1]
pose.pose.position.z = position[2]
pose.pose.orientation.w = orientation[0]
pose.pose.orientation.x = orientation[1]
pose.pose.orientation.y = orientation[2]
pose.pose.orientation.z = orientation[3]
return pose
def get_ik(self, pose, seed_state=None, xyz_bounds=None, rpy_bounds=None):
"""
get_ik returns a joint configuration from an end effector pose by using the trac_ik solver
pose: PoseStamped object. The header.frame_id should be "root", or it won't work
seed_state: a list of size 6. Initial joint positions for the solver. Default is [0,0,0,0,0,0]
xyz_bounds: a list of size 3. xyz bounds of the end effector in meters. This allows an approximate ik solution. Default is None
rpy_bounds: a list of size 3. roll pitch yaw bounds of the end effector in radians. This allows an approximate ik solution. Default is None
returns state: a list of joint values
"""
if seed_state is None:
seed_state = self.ik_default_seed
if pose.header.frame_id != self.ik_solver.base_link:
raise ValueError("Frame ID is not the root link")
position = pose.pose.position
orientation = pose.pose.orientation
print seed_state, position.x, position.y, \
position.z, orientation.x, orientation.y, \
orientation.z, orientation.w \
if xyz_bounds is None or rpy_bounds is None:
state = self.ik_solver.get_ik(seed_state, position.x, position.y,
position.z, orientation.x,
orientation.y, orientation.z,
orientation.w)
else:
state = self.ik_solver.get_ik(
seed_state, position.x, position.y, position.z, orientation.x,
orientation.y, orientation.z, orientation.w, xyz_bounds[0],
xyz_bounds[1], xyz_bounds[2], rpy_bounds[0], rpy_bounds[1],
rpy_bounds[2])
return state
def get_fk(self, joints):
"""
Gets forward kinematics to the end effector
joints: size 6 list. Joint angles for desired pose
returns pose: StackedPose of the end effector in the 'root' frame
"""
header = Header()
header.frame_id = self.ik_solver.base_link
robot_state = RobotState()
robot_state.joint_state.name = self.ik_solver.joint_names
robot_state.joint_state.position = joints
links = [self.ik_solver.tip_link]
return self.fk_solver(header, links, robot_state).pose_stamped[0]
NUM_COORDS = 200
rand_coords = []
for _ in range(NUM_COORDS):
x = random() * 0.5
y = random() * 0.6 + -0.3
z = random() * 0.7 + -0.35
rand_coords.append((x, y, z))
# Check some random coords with fixed orientation
avg_time = 0.0
num_solutions_found = 0
for x, y, z in rand_coords:
ini_t = time.time()
sol = ik_solver.get_ik(qinit,
x, y, z,
rx, ry, rz, rw,
bx, by, bz,
brx, bry, brz)
fin_t = time.time()
call_time = fin_t - ini_t
# print("IK call took: " + str(call_time))
avg_time += call_time
if sol:
# print("X, Y, Z: " + str( (x, y, z) ))
# print("SOL: " + str(sol))
num_solutions_found += 1
avg_time = avg_time / NUM_COORDS
print()
print("Found " + str(num_solutions_found) + " of 200 random coords")
print("Average IK call time: " + str(avg_time))
class GCodeGenerator():
def __init__(self):
rospy.init_node('gcode_generator', anonymous='True')
self.rospack = rospkg.RosPack()
self.ar3_path = self.rospack.get_path('ar3')
self.program_buffer = []
self.expected_len = 0
self.previous_pose = [0.0] * 6
with open(self.ar3_path + '/urdf/ar3.urdf', 'r') as fp:
urdf = fp.read()
self.solver = IK("world", "tcp", urdf_string=urdf)
def save_queue(self):
print('Saving queue.')
fp = open(self.ar3_path + '/src/ar3/programs/test.txt', 'w')
for line in self.program_buffer:
fp.write(line + '\n')
fp.close()
def queue_home_config(self):
self.expected_len += 1
home_config = [0.0] * 6
self.add_to_queue(home_config)
def queue_rest_config(self):
self.expected_len += 1
rest_angs = [0.007, 0.585, 1.020, -0.002, 1.294, 0.007]
self.add_to_queue(rest_angs)
def add_to_queue(self, angles):
program_line = ''
program_line += 'JMove'
for angle in angles:
program_line += ' %.3f' % angle
gripper_angle = 0
speed = 0.95
program_line += ' %d' % gripper_angle
program_line += ' %.2f' % speed
self.program_buffer.append(program_line)
def add_ik_to_queue(self, coord):
self.expected_len += 1
angs = self.get_ik(coord)
if angs:
self.add_to_queue(angs)
def get_ik(self, coord):
x, y, z = coord[0], coord[1], coord[2]
quat = quaternion_from_euler(coord[3], coord[4], coord[5], 'rxyz')
rx, ry, rz, rw = quat
self.qinit = self.previous_pose
angles = self.solver.get_ik(self.qinit, x, y, z, rx, ry, rz, rw)
if not angles:
print("Solution not found!")
return 0
else:
print("Solution found!")
self.previous_pose = angles
return list(angles)
ordered_joints = [
dictlist[5], dictlist[6], dictlist[3], dictlist[4], dictlist[0],
dictlist[1], dictlist[2]
]
# print ordered_joints
### SEEDING CURRENT JOINT POSITIONS INTO THE SOLVER
seed_state = ordered_joints
# seed_state = [0,0,0,0,0,0,0]
# print seed_state
# joints_from_ik1 = ik_solver.get_ik(seed_state, 0.6366221863461569, 0.8497028752234516, 0.05416354881648494, -0.3823911659266366, 0.9223729088465533, 0.023089273180701524, 0.04972020425543535)#, bx, by, bz, brx, bry, brz) # QX, QY, QZ, QW , ,
joints_from_ik1 = ik_solver.get_ik(seed_state, xl, yl, zl, Qwl, Qxl,
Qyl, Qzl, bx, by, bz, brx, bry,
brz) # QX, QY, QZ, QW , ,
# ik_solver.CartToJnt(qinit,
# x, y, z,
# rx, ry, rz, rw,
# bx, by, bz,
# brx, bry, brz)
# print "IK solver uses link chain:"
# print ik_solver.link_names
print joints_from_ik1, 'Joint Solutions'
# positions = dict(zip(left.joint_names(), list(joints_from_ik1)))
# left.move_to_joint_positions(positions, timeout=10.0, threshold=0.008726646)
# break
class boris_kinematics(object):
def __init__(self, root_link=None, tip_link="left_arm_7_link"):
self._urdf = URDF.from_parameter_server(key='robot_description')
self._kdl_tree = kdl_tree_from_urdf_model(self._urdf)
self._base_link = self._urdf.get_root(
) if root_link is None else root_link
self._tip_link = tip_link
self._arm_chain = self._kdl_tree.getChain(self._base_link,
self._tip_link)
self._joint_names = self._urdf.get_chain(self._base_link,
self._tip_link,
links=False,
fixed=False)
self._num_jnts = len(self._joint_names)
# KDL
self._fk_p_kdl = PyKDL.ChainFkSolverPos_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)
# trac_ik
urdf_str = rospy.get_param('/robot_description')
self.trac_ik_solver = IK(self._base_link,
self._tip_link,
urdf_string=urdf_str)
self.joint_limits_lower = []
self.joint_limits_upper = []
self.joint_types = []
for jnt_name in self._joint_names:
jnt = self._urdf.joint_map[jnt_name]
if jnt.limit is not None:
self.joint_limits_lower.append(jnt.limit.lower)
self.joint_limits_upper.append(jnt.limit.upper)
else:
self.joint_limits_lower.append(None)
self.joint_limits_upper.append(None)
self.joint_types.append(jnt.type)
self._default_seed = [
-0.3723750412464142, 0.02747996523976326, -0.7221865057945251,
-1.69843590259552, 0.11076358705759048, 0.5450965166091919,
0.45358774065971375
] # home_pos
#lower_lim = np.where(np.isfinite(self.joint_limits_lower), self.joint_limits_lower, 0.)
#upper_lim = np.where(np.isfinite(self.joint_limits_upper), self.joint_limits_upper, 0.)
#seed = (lower_lim + upper_lim) / 2.0
#self._default_seed = np.where(np.isnan(seed), [0.]*len(seed), seed) # mid of the joint limits
def forward_kinematics(self, joint_values):
kdl_array = joint_list_to_kdl(joint_values)
end_frame = PyKDL.Frame()
self._fk_p_kdl.JntToCart(kdl_array, end_frame)
pos = end_frame.p
quat = PyKDL.Rotation(end_frame.M).GetQuaternion()
return np.array([pos[0], pos[1], pos[2]
]), np.array([quat[0], quat[1], quat[2], quat[3]])
def trac_ik_inverse_kinematics(self, pos, quat, seed=None):
if seed is None:
seed = self._default_seed
result = self.trac_ik_solver.get_ik(seed, pos[0], pos[1], pos[2],
quat[0], quat[1], quat[2], quat[3])
# return np.array(result)
return True if result is not None else False
def trac_ik_inverse_kinematics2(self, pos, quat, seed=None):
if seed is None:
seed = self._default_seed
result = self.trac_ik_solver.get_ik(seed, pos[0], pos[1], pos[2],
quat[0], quat[1], quat[2], quat[3])
#
return np.array(result) if result is not None else None
def kdl_inverse_kinematics(self, pos, quat, seed=None):
pos = PyKDL.Vector(pos[0], pos[1], pos[2])
rot = PyKDL.Rotation()
rot = rot.Quaternion(quat[0], quat[1], quat[2], quat[3])
goal_pose = PyKDL.Frame(rot, pos)
seed_array = joint_list_to_kdl(self._default_seed)
if seed != None:
seed_array = joint_list_to_kdl(seed)
result_angles = PyKDL.JntArray(self._num_jnts)
if self._ik_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0:
result = np.array(list(result_angles))
# return result
return True
else:
# return None
return False
def kdl_inverse_kinematics_jl(self,
pos,
quat,
seed=None,
min_joints=None,
max_joints=None):
pos = PyKDL.Vector(pos[0], pos[1], pos[2])
rot = PyKDL.Rotation()
rot = rot.Quaternion(quat[0], quat[1], quat[2], quat[3])
goal_pose = PyKDL.Frame(rot, pos)
if min_joints is None:
min_joints = self.joint_limits_lower
if max_joints is None:
max_joints = self.joint_limits_upper
mins_kdl = joint_list_to_kdl(min_joints)
maxs_kdl = joint_list_to_kdl(max_joints)
ik_jl_p_kdl = PyKDL.ChainIkSolverPos_NR_JL(self._arm_chain, mins_kdl,
maxs_kdl, self._fk_p_kdl,
self._ik_v_kdl)
seed_array = joint_list_to_kdl(self._default_seed)
if seed != None:
seed_array = joint_list_to_kdl(seed)
result_angles = PyKDL.JntArray(self._num_jnts)
if ik_jl_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0:
result = np.array(list(result_angles))
# return result
return True
else:
# return None
return False
def kdl_inverse_kinematics_jl2(self,
pos,
quat,
seed=None,
min_joints=None,
max_joints=None):
pos = PyKDL.Vector(pos[0], pos[1], pos[2])
rot = PyKDL.Rotation()
rot = rot.Quaternion(quat[0], quat[1], quat[2], quat[3])
goal_pose = PyKDL.Frame(rot, pos)
if min_joints is None:
min_joints = self.joint_limits_lower
if max_joints is None:
max_joints = self.joint_limits_upper
mins_kdl = joint_list_to_kdl(min_joints)
maxs_kdl = joint_list_to_kdl(max_joints)
ik_jl_p_kdl = PyKDL.ChainIkSolverPos_NR_JL(self._arm_chain, mins_kdl,
maxs_kdl, self._fk_p_kdl,
self._ik_v_kdl)
seed_array = joint_list_to_kdl(self._default_seed)
if seed != None:
seed_array = joint_list_to_kdl(seed)
result_angles = PyKDL.JntArray(self._num_jnts)
if ik_jl_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0:
result = np.array(list(result_angles))
# return result
return result
else:
# return None
return None
class AR3Controller(QMainWindow):
def __init__(self, screen):
QMainWindow.__init__(self)
rospy.init_node('ar3_ui', anonymous='True')
self.setWindowTitle("AR3 Controller")
width = 1200
height = 900
self.setGeometry(0, 0, width, height)
self.rospack = rospkg.RosPack()
self.ar3_path = self.rospack.get_path('ar3')
uic.loadUi(self.ar3_path + '/src/ar3/ui/ar3_controller.ui', self)
self.joint_jog_widget = uic.loadUi(self.ar3_path +
'/src/ar3/ui/joint_controller.ui')
self.pose_jog_widget = uic.loadUi(self.ar3_path +
'/src/ar3/ui/pose_controller.ui')
with open(self.ar3_path + '/urdf/ar3.urdf', 'r') as fp:
urdf = fp.read()
self.solver = IK("world", "tcp", urdf_string=urdf)
self.qinit = [0.0] * self.solver.number_of_joints
self.connected = False
self.ar3_feeback_sub = rospy.Subscriber('/AR3/Feedback', ar3_feedback,
self.update_feedback_label)
self.listener = tf.TransformListener()
self.robot_controller = RobotController()
self.robot_controller.AR3Control.run = 1
self.robot_controller.AR3Control.accelerate = 1
self.feedback_angles = []
self.setpoint_angles = []
self.gripper_angle = self.gripper_angle_spinbox.value()
# Move Commands
self.move_to_pose_button.clicked.connect(self.move_to_pose)
self.move_to_rest_button.clicked.connect(self.move_to_rest)
self.move_to_home_button.clicked.connect(self.move_to_home)
self.move_gripper_button.clicked.connect(self.move_gripper)
self.pull_current_config_button.clicked.connect(
self.pull_current_config)
self.joint_spinboxes = [
self.joint_jog_widget.joint_1_setpoint,
self.joint_jog_widget.joint_2_setpoint,
self.joint_jog_widget.joint_3_setpoint,
self.joint_jog_widget.joint_4_setpoint,
self.joint_jog_widget.joint_5_setpoint,
self.joint_jog_widget.joint_6_setpoint
]
self.pose_spinboxes = [
self.pose_jog_widget.x_spinbox, self.pose_jog_widget.y_spinbox,
self.pose_jog_widget.z_spinbox, self.pose_jog_widget.rx_spinbox,
self.pose_jog_widget.ry_spinbox, self.pose_jog_widget.rz_spinbox
]
# Program Operations
self.program_buffer = []
self.cursor_idx = -1
self.add_to_queue_button.clicked.connect(self.add_to_queue)
self.start_queue_button.clicked.connect(self.start_queue)
self.clear_queue_button.clicked.connect(self.clear_queue)
self.program_up_button.clicked.connect(self.program_up)
self.program_down_button.clicked.connect(self.program_down)
self.program_remove_button.clicked.connect(self.program_remove)
self.add_program_wait_button.clicked.connect(self.add_wait_to_queue)
# Jog Panel
self.timer = QTimer()
self.timer.timeout.connect(self.jog_joints)
self.jog_rate = 200
self.jog_type = 0
self.joint_jog_idx = 0
self.joint_jog_dir = 1.0
self.joint_radiobutton.toggled.connect(self.set_jog_type)
self.pose_radiobutton.toggled.connect(self.set_jog_type)
self.jog_layout.addWidget(self.joint_jog_widget)
self.jog_layout.addWidget(self.pose_jog_widget)
self.pose_jog_widget.hide()
self.cartesian_jog_radiobutton.toggled.connect(self.switch_jog_type)
self.joint_jog_radiobutton.toggled.connect(self.switch_jog_type)
self.ax_labels = [
self.ax1_label, self.ax2_label, self.ax3_label, self.ax4_label,
self.ax5_label, self.ax6_label
]
self.jog_x_pos_button.pressed.connect(self.start_jog_x_pos)
self.jog_x_pos_button.released.connect(self.stop_jogging)
self.jog_x_neg_button.pressed.connect(self.start_jog_x_neg)
self.jog_x_neg_button.released.connect(self.stop_jogging)
self.jog_y_pos_button.pressed.connect(self.start_jog_y_pos)
self.jog_y_pos_button.released.connect(self.stop_jogging)
self.jog_y_neg_button.pressed.connect(self.start_jog_y_neg)
self.jog_y_neg_button.released.connect(self.stop_jogging)
self.jog_z_pos_button.pressed.connect(self.start_jog_z_pos)
self.jog_z_pos_button.released.connect(self.stop_jogging)
self.jog_z_neg_button.pressed.connect(self.start_jog_z_neg)
self.jog_z_neg_button.released.connect(self.stop_jogging)
self.jog_rx_pos_button.pressed.connect(self.start_jog_rx_pos)
self.jog_rx_pos_button.released.connect(self.stop_jogging)
self.jog_rx_neg_button.pressed.connect(self.start_jog_rx_neg)
self.jog_rx_neg_button.released.connect(self.stop_jogging)
self.jog_ry_pos_button.pressed.connect(self.start_jog_ry_pos)
self.jog_ry_pos_button.released.connect(self.stop_jogging)
self.jog_ry_neg_button.pressed.connect(self.start_jog_ry_neg)
self.jog_ry_neg_button.released.connect(self.stop_jogging)
self.jog_rz_pos_button.pressed.connect(self.start_jog_rz_pos)
self.jog_rz_pos_button.released.connect(self.stop_jogging)
self.jog_rz_neg_button.pressed.connect(self.start_jog_rz_neg)
self.jog_rz_neg_button.released.connect(self.stop_jogging)
self.actionSave_Queue.triggered.connect(self.save_queue)
self.actionLoad_Queue.triggered.connect(self.load_queue)
# Feedback Labels
self.tcp_pose = None
self.joint_lcds = [
self.j1_lcd, self.j2_lcd, self.j3_lcd, self.j4_lcd, self.j5_lcd,
self.j6_lcd
]
self.tcp_lcds = [
self.x_lcd, self.y_lcd, self.z_lcd, self.rx_lcd, self.ry_lcd,
self.rz_lcd
]
self.show()
self.set_jog_type()
self.switch_jog_type()
if not self.connected:
self.frame_3.setEnabled(False)
'''
Menu Bar Actions
'''
def save_queue(self):
print('Saving queue...')
fp = open(self.ar3_path + '/src/ar3/programs/test.txt', 'w')
for line in self.program_buffer:
fp.write(line + '\n')
fp.close()
def load_queue(self):
print('Loading queue...')
self.clear_queue()
fp = open(self.ar3_path + '/src/ar3/programs/test.txt', 'r')
for line in fp:
line = line.strip('\n')
self.add_to_queue(from_buffer=line)
fp.close()
'''
Jog Controls
'''
def switch_jog_type(self):
if self.cartesian_jog_radiobutton.isChecked():
self.jog_type = 1
keys = ['X:', 'Y:', 'Z:', 'RX:', 'RY:', 'RZ:']
for idx in range(6):
self.ax_labels[idx].setText(keys[idx])
else:
self.jog_type = 0
for idx in range(6):
self.ax_labels[idx].setText('J' + str(idx + 1) + ":")
# Cartesian X
def start_jog_x_neg(self):
self.set_jog_params(0, -1.0)
def start_jog_x_pos(self):
self.set_jog_params(0, 1.0)
# Cartesian Y
def start_jog_y_neg(self):
self.set_jog_params(1, -1.0)
def start_jog_y_pos(self):
self.set_jog_params(1, 1.0)
# Catesian Z
def start_jog_z_neg(self):
self.set_jog_params(2, -1.0)
def start_jog_z_pos(self):
self.set_jog_params(2, 1.0)
# Cartesian RX
def start_jog_rx_neg(self):
self.set_jog_params(3, -1.0)
def start_jog_rx_pos(self):
self.set_jog_params(3, 1.0)
# Cartesian RY
def start_jog_ry_neg(self):
self.set_jog_params(4, -1.0)
def start_jog_ry_pos(self):
self.set_jog_params(4, 1.0)
# Cartesian RZ
def start_jog_rz_neg(self):
self.set_jog_params(5, -1.0)
def start_jog_rz_pos(self):
self.set_jog_params(5, 1.0)
def set_jog_params(self, idx, ax_dir):
if self.jog_type == 1:
self.joint_jog_idx = idx
self.joint_jog_dir = ax_dir
self.starting_pose = list(self.tcp_pose)
self.jog_step = 1
self.timer.start(self.jog_rate)
elif self.jog_type == 0:
self.joint_jog_idx = idx
self.joint_jog_dir = ax_dir
self.timer.start(self.jog_rate)
def stop_jogging(self):
self.timer.stop()
self.stop_motion()
def stop_motion(self):
self.robot_controller.AR3Control.joint_angles = self.feedback_angles
self.robot_controller.send_joints()
def jog_joints(self):
if self.jog_type == 1:
pose = self.starting_pose
quat = quaternion_from_euler(pose[3], pose[4], pose[5], 'rxyz')
rx, ry, rz, rw = quat
pose[self.joint_jog_idx] += (.001 *
self.jog_step) * self.joint_jog_dir
x, y, z, = pose[0], pose[1], pose[2]
self.qinit = list(self.feedback_angles)
angles = self.solver.get_ik(self.qinit, x, y, z, rx, ry, rz, rw)
if not angles:
print("Solution not found!")
return
angles = list(angles)
self.jog_step += 1
else:
angles = list(self.feedback_angles)
angles[self.joint_jog_idx] += .2 * self.joint_jog_dir
self.gripper_angle = self.robot_controller.AR3Feedback.gripper_angle
self.speed = self.speed_spinbox.value()
self.robot_controller.AR3Control.accelerate = 1
self.robot_controller.AR3Control.speed = self.speed
self.robot_controller.AR3Control.gripper_angle = self.gripper_angle
self.robot_controller.AR3Control.joint_angles = angles
self.robot_controller.send_joints()
'''
Program Controls
'''
def program_up(self):
if self.queue_list.count() == 0:
return
self.cursor_idx = self.queue_list.currentRow()
program_line = self.queue_list.takeItem(self.cursor_idx)
self.program_buffer.pop(self.cursor_idx)
self.cursor_idx -= 1
if self.cursor_idx < 0:
self.cursor_idx = 0
self.queue_list.insertItem(self.cursor_idx, program_line)
self.program_buffer.insert(self.cursor_idx, program_line)
self.queue_list.setCurrentRow(self.cursor_idx)
def program_down(self):
if self.queue_list.count() == 0:
return
count = self.queue_list.count()
self.cursor_idx = self.queue_list.currentRow()
program_line = self.queue_list.takeItem(self.cursor_idx)
self.program_buffer.pop(self.cursor_idx)
self.cursor_idx += 1
if self.cursor_idx >= count:
self.cursor_idx = count - 1
self.queue_list.insertItem(self.cursor_idx, program_line)
self.program_buffer.insert(self.cursor_idx, program_line)
self.queue_list.setCurrentRow(self.cursor_idx)
def program_remove(self):
if self.queue_list.count() == 0:
return
self.cursor_idx = self.queue_list.currentRow()
self.queue_list.takeItem(self.cursor_idx)
self.program_buffer.pop(self.cursor_idx)
count = self.queue_list.count()
if self.cursor_idx > count - 1:
self.cursor_idx = count - 1
def add_wait_to_queue(self):
delay = self.program_wait_spinbox.value()
program_line = ''
program_line += 'Wait'
program_line += ' %0.2f' % (delay)
self.program_buffer.append(program_line)
self.cursor_idx += 1
self.queue_list.insertItem(self.cursor_idx, program_line)
self.queue_list.setCurrentRow(self.cursor_idx)
print("Queueing: {}".format(program_line))
def add_to_queue(self, from_buffer=None):
if from_buffer:
program_line = from_buffer
else:
program_line = ''
program_line += 'JMove'
for angle in self.feedback_angles:
program_line += ' %.3f' % angle
program_line += ' %d' % self.robot_controller.AR3Feedback.gripper_angle
program_line += ' %.2f' % self.speed_spinbox.value()
self.program_buffer.append(program_line)
self.cursor_idx += 1
self.queue_list.insertItem(self.cursor_idx, program_line)
self.queue_list.setCurrentRow(self.cursor_idx)
print("Queueing: {}".format(program_line))
def clear_queue(self):
self.queue_list.clear()
self.program_buffer = []
def wait_for_move(self):
flag = True
while flag:
flag = False
for idx in range(0, 6):
if abs(self.setpoint_angles[idx] -
self.feedback_angles[idx]) > 0.01:
flag = True
if abs(self.gripper_angle - self.gripper_feedback) >= 1.0:
flag = True
def run_queue(self):
print('Running queue with ' + str(self.queue_list.count()) + ' lines.')
for idx in range(self.queue_list.count()):
program_line = self.queue_list.item(idx).text()
tokens = program_line.split(' ')
if tokens[0] == 'Wait':
time.sleep(float(tokens[1]))
if tokens[0] == 'JMove':
angles = tokens[1:7]
for n in range(0, 6):
angles[n] = float(angles[n])
gripper_val = int(tokens[7])
speed = float(tokens[8])
self.setpoint_angles = angles
print("Moving to: {} {} {}".format(angles, gripper_val, speed))
self.robot_controller.AR3Control.accelerate = 1
self.robot_controller.AR3Control.speed = speed
self.robot_controller.AR3Control.gripper_angle = gripper_val
self.robot_controller.AR3Control.joint_angles = angles
self.robot_controller.send_joints()
self.wait_for_move()
else:
print('Unrecognized command!')
print("Program completed.")
def start_queue(self):
if self.queue_list.count() == 0:
return
self.queue_thread = threading.Thread(target=self.run_queue)
self.queue_thread.start()
'''
Move to Pose Controls
'''
def pull_current_config(self):
idx = 0
for spinbox in self.joint_spinboxes:
spinbox.setValue(self.feedback_angles[idx])
idx += 1
(trans, rot) = self.listener.lookupTransform('/world', '/tcp',
rospy.Time(0))
rot = euler_from_quaternion(rot, 'rxyz')
self.tcp_pose = list(trans) + list(rot)
idx = 0
for spinbox in self.pose_spinboxes:
spinbox.setValue(self.tcp_pose[idx])
idx += 1
self.gripper_angle_spinbox.setValue(
int(self.robot_controller.AR3Feedback.gripper_angle))
def set_jog_type(self):
if self.joint_radiobutton.isChecked():
self.joint_jog_widget.show()
self.pose_jog_widget.hide()
if self.pose_radiobutton.isChecked():
self.pose_jog_widget.show()
self.joint_jog_widget.hide()
def move_gripper(self):
self.gripper_angle = self.gripper_angle_spinbox.value()
self.robot_controller.AR3Control.gripper_angle = self.gripper_angle
self.robot_controller.send_joints()
def move_to_pose(self):
self.robot_controller.AR3Control.accelerate = 1
if self.joint_radiobutton.isChecked():
angles = []
for spinbox in self.joint_spinboxes:
angles.append(spinbox.value())
self.gripper_angle = self.gripper_angle_spinbox.value()
self.speed = self.speed_spinbox.value()
self.robot_controller.AR3Control.speed = self.speed
self.robot_controller.AR3Control.gripper_angle = self.gripper_angle
self.robot_controller.AR3Control.joint_angles = angles
self.robot_controller.send_joints()
elif self.pose_radiobutton.isChecked():
coord = []
for spinbox in self.pose_spinboxes:
coord.append(spinbox.value())
quat = quaternion_from_euler(coord[3], coord[4], coord[5], 'rxyz')
rx, ry, rz, rw = quat
x, y, z = coord[0], coord[1], coord[2]
self.qinit = self.feedback_angles
angles = self.solver.get_ik(self.qinit, x, y, z, rx, ry, rz, rw)
print("Requested solution for: {} {} {} {} {} {}".format(
x, y, z, coord[3], coord[4], coord[5]))
if not angles:
print("Solution not found!")
return
else:
angles = list(angles)
print("Solution found!")
self.gripper_angle = self.gripper_angle_spinbox.value()
self.speed = self.speed_spinbox.value()
self.robot_controller.AR3Control.speed = self.speed
self.robot_controller.AR3Control.gripper_angle = self.gripper_angle
self.robot_controller.AR3Control.joint_angles = angles
self.robot_controller.send_joints()
def move_to_rest(self):
self.gripper_angle = self.gripper_angle_spinbox.value()
self.speed = self.speed_spinbox.value()
self.robot_controller.AR3Control.speed = self.speed
self.robot_controller.AR3Control.gripper_angle = self.gripper_angle
self.robot_controller.AR3Control.joint_angles = [
0.0, 0.0, 1.57, 0.0, 1.4, 0.0
]
self.robot_controller.send_joints()
def move_to_home(self):
self.gripper_angle = self.gripper_angle_spinbox.value()
self.speed = self.speed_spinbox.value()
self.robot_controller.AR3Control.speed = self.speed
self.robot_controller.AR3Control.gripper_angle = self.gripper_angle
self.robot_controller.AR3Control.joint_angles = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
self.robot_controller.send_joints()
'''
Feedback Panel
'''
def update_feedback_label(self, data):
self.connected = True
self.frame_3.setEnabled(True)
try:
idx = 0
for lcd in self.joint_lcds:
lcd.display('%0.3f' % data.joint_angles[idx])
idx += 1
self.feedback_angles = data.joint_angles
self.ar3_feedback = data
(trans,
rot) = self.listener.lookupTransform('/world', '/tcp',
rospy.Time(0))
rot = euler_from_quaternion(rot, 'rxyz')
self.tcp_pose = list(trans) + list(rot)
idx = 0
for lcd in self.tcp_lcds:
lcd.display('%0.3f' % self.tcp_pose[idx])
idx += 1
self.gripper_lcd.display(data.gripper_angle)
self.gripper_feedback = data.gripper_angle
if data.eStop:
self.state_label.setText("E-STOP PRESSED")
elif data.running:
self.state_label.setText("RUNNING")
except:
print('Failed to update the current config panel.')
class Compensation():
def __init__(self, Fg=[90.06, -42.79, -52.23], F_thresh=10.0, K=0.015):
rospy.init_node('ft_feedback_control')
self.Fg = np.array(Fg)
self.F_thresh = F_thresh
self.K = K
self.Tmax = 2.0
self.js = None
self.wind_up = 0
self.IK = IK('ceiling',
'blue_robotiq_ft_frame_id',
solve_type='Distance')
self.robot = moveit_commander.RobotCommander()
self.group = moveit_commander.MoveGroupCommander('blue_arm')
self.listener = tf.TransformListener()
rospy.Subscriber("/blue/joint_states", JointState, self.joint_callback)
rospy.Subscriber("/robotiq_ft_wrench",
WrenchStamped,
self.ft_callback,
queue_size=1)
self.client = actionlib.SimpleActionClient(
'blue/follow_joint_trajectory', FollowJointTrajectoryAction)
rospy.spin()
def joint_callback(self, data):
self.js = np.array(data.position)
self.js_names = data.name
def ft_callback(self, data):
ft = np.array(
[data.wrench.force.x, data.wrench.force.y, data.wrench.force.z])
self.wind_up += 1
self.wind_up = min(self.wind_up, 100)
df = np.sum(np.abs(ft - self.Fg))
# print('FT dev',df,self.wind_up)
if (df > self.F_thresh) and (self.wind_up > 50):
if self.js is not None:
self.listener.waitForTransform(self.group.get_planning_frame(),
"/blue_robotiq_ft_frame_id",
data.header.stamp,
rospy.Duration(4.0))
p = PointStamped()
p.header = data.header
p.point.x = self.K * (ft - self.Fg)[0]
p.point.y = self.K * (ft - self.Fg)[1]
p.point.z = self.K * (ft - self.Fg)[2]
pose = self.group.get_current_pose().pose
print('New force deviation:')
print(ft, self.Fg)
print(p.point.x, p.point.y, p.point.z)
pnew = self.listener.transformPoint('ceiling', p)
print('Original pose:', pose.position.z, pose.position.x,
pose.position.y)
pose.position.z = pnew.point.z
pose.position.y = pnew.point.y
pose.position.x = pnew.point.x
print('New pose', pose.position.z, pose.position.x,
pose.position.y)
sol = None
retries = 0
while not sol and retries < 20:
sol = self.IK.get_ik(self.js, pose.position.x,
pose.position.y, pose.position.z,
pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w)
retries += 1
if retries < 20:
print(sol)
print(self.js)
print(np.sum((self.js - np.array(sol))**2))
if (np.sum((self.js - np.array(sol))**2) < 0.5):
self.publish(sol)
self.wind_up = 0
else:
self.Fg = 0.1 * ft + 0.9 * self.Fg
def publish(self, q):
if q is not None:
goal = FollowJointTrajectoryGoal()
jtp = JointTrajectoryPoint()
jtp.positions = list(q)
jtp.velocities = [0.0] * len(jtp.positions)
jtp.time_from_start = rospy.Time(self.Tmax)
goal.trajectory.points.append(jtp)
goal.trajectory.header.stamp = rospy.Time.now()
goal.trajectory.joint_names = self.js_names
self.client.send_goal(goal)
self.client.wait_for_result()
from trajectory_msgs.msg import JointTrajectoryPoint
import rospy
from trac_ik_python.trac_ik import IK
from tf.transformations import quaternion_from_euler
ik_solver = IK("base_link", "wrist_2_link")
seed_state = [0.0] * ik_solver.number_of_joints
# Convert RPY to Quaternions
q = quaternion_from_euler(0, 0, 0)
joint_space = ik_solver.get_ik(seed_state,
0.0, 0.50, 0.3, # X, Y, Z
q[0], q[1], q[2], q[3]) # QX, QY, QZ, QW
print(joint_space)
waypoints = [[joint_space[0], joint_space[1], joint_space[2], joint_space[3], joint_space[4], 0]]
def main():
rospy.init_node('send_joints')
pub = rospy.Publisher('/arm_controller/command',
JointTrajectory,
queue_size=10)
# Create the topic message
traj = JointTrajectory()
traj.header = Header()
class Rbx1_Kinematics(object):
def __init__(
self
): # gets URDF from /robot_description parameter on param server
rospy.init_node('trac_ik_py')
rospy.loginfo("Kinematic thingy initializin' here...")
self.ik_solver = IK("base_link", "Link_6")
self.seed_state = [0.0] * self.ik_solver.number_of_joints
self.publisher = rospy.Publisher('/joint_states',
JointState,
queue_size=10)
self.seq = 0
self.z_offset = 0.1575
self.scale = 1
self.x = self.y = self.z = self.qx = self.qy = self.qz = self.qw = 0 # keep track of EE pose
def move(self, x, y, z, qx, qy, qz, qw):
print("{} {} {} {} {} {} {}".format(x, y, z, qx, qy, qz, qw))
joint_states = self.ik_solver.get_ik(self.seed_state, x, y, z, qx, qy,
qz, qw)
print(joint_states)
if joint_states == None:
raise ValueError("Can't reach target.")
hdr = Header()
hdr.seq = self.seq = self.seq + 1
hdr.stamp = rospy.Time.now()
hdr.frame_id = "trac-ik-py"
js = JointState()
js.header = hdr
js.name = [
"Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6"
] #, , , "Joint_Gripper"
js.position = joint_states
js.velocity = []
js.effort = []
self.publisher.publish(js)
# since we got moved the robot, let's keep track of current EE pose
self.x = x
self.y = y
self.z = z
self.qx = qx
self.qy = qy
self.qz = qz
self.qw = qw
def callback(self, data):
# print(data)
self.move(data.pose.position.x + (self.x * self.scale),
data.pose.position.y + (self.y * self.scale),
data.pose.position.z + (self.z * self.scale), self.qx,
self.qy, self.qz, self.qw)
# NOTE: ignoring orientation because I want EE to stay straight down!
# data.pose.orientation.x + (self.qx * self.scale),
# data.pose.orientation.y + (self.qy * self.scale),
# data.pose.orientation.z + (self.qz * self.scale),
# data.pose.orientation.w + (self.qw * self.scale))
def listener(self):
rospy.Subscriber("/unity/controllers/left", PoseStamped, self.callback)
rospy.spin()
class World(object):
def __init__(self,
robot_name=FRANKA_CARTER,
use_gui=True,
full_kitchen=False):
self.task = None
self.interface = None
self.client = connect(use_gui=use_gui)
set_real_time(False)
#set_caching(False) # Seems to make things worse
disable_gravity()
add_data_path()
set_camera_pose(camera_point=[2, -1.5, 1])
if DEBUG:
draw_pose(Pose(), length=3)
#self.kitchen_yaml = load_yaml(KITCHEN_YAML)
with HideOutput(enable=True):
self.kitchen = load_pybullet(KITCHEN_PATH,
fixed_base=True,
cylinder=True)
self.floor = load_pybullet('plane.urdf', fixed_base=True)
z = stable_z(self.kitchen, self.floor) - get_point(self.floor)[2]
point = np.array(get_point(self.kitchen)) - np.array([0, 0, z])
set_point(self.floor, point)
self.robot_name = robot_name
if self.robot_name == FRANKA_CARTER:
urdf_path, yaml_path = FRANKA_CARTER_PATH, None
#urdf_path, yaml_path = FRANKA_CARTER_PATH, FRANKA_YAML
#elif self.robot_name == EVE:
# urdf_path, yaml_path = EVE_PATH, None
else:
raise ValueError(self.robot_name)
self.robot_yaml = yaml_path if yaml_path is None else load_yaml(
yaml_path)
with HideOutput(enable=True):
self.robot = load_pybullet(urdf_path)
#dump_body(self.robot)
#chassis_pose = get_link_pose(self.robot, link_from_name(self.robot, 'chassis_link'))
#wheel_pose = get_link_pose(self.robot, link_from_name(self.robot, 'left_wheel_link'))
#wait_for_user()
set_point(self.robot, Point(z=stable_z(self.robot, self.floor)))
self.set_initial_conf()
self.gripper = create_gripper(self.robot)
self.environment_bodies = {}
if full_kitchen:
self._initialize_environment()
self._initialize_ik(urdf_path)
self.initial_saver = WorldSaver()
self.body_from_name = {}
# self.path_from_name = {}
self.names_from_type = {}
self.custom_limits = {}
self.base_limits_handles = []
self.cameras = {}
self.disabled_collisions = set()
if self.robot_name == FRANKA_CARTER:
self.disabled_collisions.update(
tuple(link_from_name(self.robot, link) for link in pair)
for pair in DISABLED_FRANKA_COLLISIONS)
self.carry_conf = FConf(self.robot, self.arm_joints, self.default_conf)
#self.calibrate_conf = Conf(self.robot, self.arm_joints, load_calibrate_conf(side='left'))
self.calibrate_conf = FConf(
self.robot, self.arm_joints,
self.default_conf) # Must differ from carry_conf
self.special_confs = [self.carry_conf] #, self.calibrate_conf]
self.open_gq = FConf(self.robot, self.gripper_joints,
get_max_limits(self.robot, self.gripper_joints))
self.closed_gq = FConf(self.robot, self.gripper_joints,
get_min_limits(self.robot, self.gripper_joints))
self.gripper_confs = [self.open_gq, self.closed_gq]
self.open_kitchen_confs = {
joint: FConf(self.kitchen, [joint], [self.open_conf(joint)])
for joint in self.kitchen_joints
}
self.closed_kitchen_confs = {
joint: FConf(self.kitchen, [joint], [self.closed_conf(joint)])
for joint in self.kitchen_joints
}
self._update_custom_limits()
self._update_initial()
def _initialize_environment(self):
# wall to fridge: 4cm
# fridge to goal: 1.5cm
# hitman to range: 3.5cm
# range to indigo: 3.5cm
self.environment_poses = read_json(POSES_PATH)
root_from_world = get_link_pose(self.kitchen, self.world_link)
for name, world_from_part in self.environment_poses.items():
if name in ['range']:
continue
visual_path = os.path.join(KITCHEN_LEFT_PATH,
'{}.obj'.format(name))
collision_path = os.path.join(KITCHEN_LEFT_PATH,
'{}_collision.obj'.format(name))
mesh_path = None
for path in [collision_path, visual_path]:
if os.path.exists(path):
mesh_path = path
break
if mesh_path is None:
continue
body = load_pybullet(mesh_path, fixed_base=True)
root_from_part = multiply(root_from_world, world_from_part)
if name in ['axe', 'dishwasher', 'echo', 'fox', 'golf']:
(pos, quat) = root_from_part
# TODO: still not totally aligned
pos = np.array(pos) + np.array([0, -0.035, 0]) # , -0.005])
root_from_part = (pos, quat)
self.environment_bodies[name] = body
set_pose(body, root_from_part)
# TODO: release bounding box or convex hull
# TODO: static object nonconvex collisions
if TABLE_NAME in self.environment_bodies:
body = self.environment_bodies[TABLE_NAME]
_, (w, l, _) = approximate_as_prism(body)
_, _, z = get_point(body)
new_pose = Pose(Point(TABLE_X + l / 2, -TABLE_Y, z),
Euler(yaw=np.pi / 2))
set_pose(body, new_pose)
def _initialize_ik(self, urdf_path):
if not USE_TRACK_IK:
self.ik_solver = None
return
from trac_ik_python.trac_ik import IK # killall -9 rosmaster
base_link = get_link_name(
self.robot, parent_link_from_joint(self.robot, self.arm_joints[0]))
tip_link = get_link_name(self.robot,
child_link_from_joint(self.arm_joints[-1]))
# limit effort and velocities are required
# solve_type: Speed, Distance, Manipulation1, Manipulation2
# TODO: fast solver and slow solver
self.ik_solver = IK(base_link=str(base_link),
tip_link=str(tip_link),
timeout=0.01,
epsilon=1e-5,
solve_type="Speed",
urdf_string=read(urdf_path))
if not CONSERVITIVE_LIMITS:
return
lower, upper = self.ik_solver.get_joint_limits()
buffer = JOINT_LIMITS_BUFFER * np.ones(len(self.ik_solver.joint_names))
lower, upper = lower + buffer, upper - buffer
lower[6] = -MAX_FRANKA_JOINT7
upper[6] = +MAX_FRANKA_JOINT7
self.ik_solver.set_joint_limits(lower, upper)
def _update_initial(self):
# TODO: store initial poses as well?
self.initial_saver = WorldSaver()
self.goal_bq = FConf(self.robot, self.base_joints)
self.goal_aq = FConf(self.robot, self.arm_joints)
if are_confs_close(self.goal_aq, self.carry_conf):
self.goal_aq = self.carry_conf
self.goal_gq = FConf(self.robot, self.gripper_joints)
self.initial_confs = [self.goal_bq, self.goal_aq, self.goal_gq]
set_all_static()
def is_real(self):
return (self.task is not None) and self.task.real
@property
def constants(self):
return self.special_confs + self.gripper_confs + self.initial_confs
#########################
@property
def base_joints(self):
return joints_from_names(self.robot, BASE_JOINTS)
@property
def arm_joints(self):
#if self.robot_name == EVE:
# return get_eve_arm_joints(self.robot, arm=DEFAULT_ARM)
joint_names = ['panda_joint{}'.format(1 + i) for i in range(7)]
#joint_names = self.robot_yaml['cspace']
return joints_from_names(self.robot, joint_names)
@property
def gripper_joints(self):
#if self.robot_yaml is None:
# return []
joint_names = ['panda_finger_joint{}'.format(1 + i) for i in range(2)]
#joint_names = [joint_from_name(self.robot, rule['name'])
# for rule in self.robot_yaml['cspace_to_urdf_rules']]
return joints_from_names(self.robot, joint_names)
@property
def kitchen_joints(self):
joint_names = get_joint_names(self.kitchen,
get_movable_joints(self.kitchen))
#joint_names = self.kitchen_yaml['cspace']
#return joints_from_names(self.kitchen, joint_names)
return joints_from_names(self.kitchen,
filter(ALL_JOINTS.__contains__, joint_names))
@property
def base_link(self):
return child_link_from_joint(self.base_joints[-1])
@property
def franka_link(self):
return parent_link_from_joint(self.robot, self.arm_joints[0])
@property
def gripper_link(self):
return parent_link_from_joint(self.robot, self.gripper_joints[0])
@property
def tool_link(self):
return link_from_name(self.robot, get_tool_link(self.robot))
@property
def world_link(self): # for kitchen
return BASE_LINK
@property
def door_links(self):
door_links = set()
for joint in self.kitchen_joints:
door_links.update(get_link_subtree(self.kitchen, joint))
return door_links
@property
def static_obstacles(self):
# link=None is fine
# TODO: decompose obstacles
#return [(self.kitchen, frozenset(get_links(self.kitchen)) - self.door_links)]
return {(self.kitchen, frozenset([link])) for link in
set(get_links(self.kitchen)) - self.door_links} | \
{(body, None) for body in self.environment_bodies.values()}
@property
def movable(self): # movable base
return set(self.body_from_name) # frozenset?
@property
def fixed(self): # fixed base
return set(self.environment_bodies.values()) | {self.kitchen}
@property
def all_bodies(self):
return self.movable | self.fixed | {self.robot}
@property
def default_conf(self):
# if self.robot_name == EVE:
# # Eve starts outside of joint limits
# # Eve starts outside of joint limits
# conf = [np.average(get_joint_limits(self.robot, joint)) for joint in self.arm_joints]
# #conf = np.zeros(len(self.arm_joints))
# #conf[3] -= np.pi / 2
# return conf
return DEFAULT_ARM_CONF
#conf = np.array(self.robot_yaml['default_q'])
##conf[1] += np.pi / 4
##conf[3] -= np.pi / 4
#return conf
#########################
# TODO: could perform base motion planning without free joints
def get_base_conf(self):
return get_joint_positions(self.robot, self.base_joints)
def set_base_conf(self, conf):
set_joint_positions(self.robot, self.base_joints, conf)
def get_base_aabb(self):
franka_links = get_link_subtree(self.robot, self.franka_link)
base_links = get_link_subtree(self.robot, self.base_link)
return aabb_union(
get_aabb(self.robot, link)
for link in set(base_links) - set(franka_links))
def get_world_aabb(self):
return aabb_union(get_aabb(body)
for body in self.fixed) # self.all_bodies
def _update_custom_limits(self, min_extent=0.0):
#robot_extent = get_aabb_extent(get_aabb(self.robot))
# Scaling by 0.5 to prevent getting caught in corners
#min_extent = 0.5 * min(robot_extent[:2]) * np.ones(2) / 2
world_aabb = self.get_world_aabb()
full_lower, full_upper = world_aabb
base_limits = (full_lower[:2] - min_extent,
full_upper[:2] + min_extent)
base_limits[1][0] = COMPUTER_X - min_extent # TODO: robot radius
base_limits[0][1] += 0.1
base_limits[1][1] -= 0.1
for handle in self.base_limits_handles:
remove_debug(handle)
self.base_limits_handles = []
#self.base_limits_handles.extend(draw_aabb(world_aabb))
z = get_point(self.floor)[2] + 1e-2
if DEBUG:
self.base_limits_handles.extend(draw_base_limits(base_limits, z=z))
self.custom_limits = custom_limits_from_base_limits(
self.robot, base_limits)
return self.custom_limits
def solve_trac_ik(self, world_from_tool, nearby_tolerance=INF):
assert self.ik_solver is not None
init_lower, init_upper = self.ik_solver.get_joint_limits()
base_link = link_from_name(self.robot, self.ik_solver.base_link)
world_from_base = get_link_pose(self.robot, base_link)
tip_link = link_from_name(self.robot, self.ik_solver.tip_link)
tool_from_tip = multiply(
invert(get_link_pose(self.robot, self.tool_link)),
get_link_pose(self.robot, tip_link))
world_from_tip = multiply(world_from_tool, tool_from_tip)
base_from_tip = multiply(invert(world_from_base), world_from_tip)
joints = joints_from_names(
self.robot,
self.ik_solver.joint_names) # self.ik_solver.link_names
seed_state = get_joint_positions(self.robot, joints)
# seed_state = [0.0] * self.ik_solver.number_of_joints
lower, upper = init_lower, init_upper
if nearby_tolerance < INF:
tolerance = nearby_tolerance * np.ones(len(joints))
lower = np.maximum(lower, seed_state - tolerance)
upper = np.minimum(upper, seed_state + tolerance)
self.ik_solver.set_joint_limits(lower, upper)
(x, y, z), (rx, ry, rz, rw) = base_from_tip
# TODO: can also adjust tolerances
conf = self.ik_solver.get_ik(seed_state, x, y, z, rx, ry, rz, rw)
self.ik_solver.set_joint_limits(init_lower, init_upper)
if conf is None:
return conf
# if nearby_tolerance < INF:
# print(lower.round(3))
# print(upper.round(3))
# print(conf)
# print(get_difference(seed_state, conf).round(3))
set_joint_positions(self.robot, joints, conf)
return get_configuration(self.robot)
def solve_pybullet_ik(self, world_from_tool, nearby_tolerance):
start_time = time.time()
# Most of the time is spent creating the robot
# TODO: use the waypoint version that doesn't repeatedly create the robot
current_conf = get_joint_positions(self.robot, self.arm_joints)
full_conf = sub_inverse_kinematics(
self.robot,
self.arm_joints[0],
self.tool_link,
world_from_tool,
custom_limits=self.custom_limits
) # , max_iterations=1) # , **kwargs)
conf = get_joint_positions(self.robot, self.arm_joints)
max_distance = get_distance(current_conf, conf, norm=INF)
if nearby_tolerance < max_distance:
return None
print('Nearby) time: {:.3f} | distance: {:.5f}'.format(
elapsed_time(start_time), max_distance))
return full_conf
def solve_inverse_kinematics(self,
world_from_tool,
nearby_tolerance=INF,
**kwargs):
if self.ik_solver is not None:
return self.solve_trac_ik(world_from_tool, **kwargs)
#if nearby_tolerance != INF:
# return self.solve_pybullet_ik(world_from_tool, nearby_tolerance=nearby_tolerance)
current_conf = get_joint_positions(self.robot, self.arm_joints)
start_time = time.time()
if nearby_tolerance == INF:
generator = ikfast_inverse_kinematics(self.robot,
PANDA_INFO,
self.tool_link,
world_from_tool,
max_attempts=10,
use_halton=True)
else:
generator = closest_inverse_kinematics(
self.robot,
PANDA_INFO,
self.tool_link,
world_from_tool,
max_time=0.01,
max_distance=nearby_tolerance,
use_halton=True)
conf = next(generator, None)
#conf = sample_tool_ik(self.robot, world_from_tool, max_attempts=100)
if conf is None:
return conf
max_distance = get_distance(current_conf, conf, norm=INF)
#print('Time: {:.3f} | distance: {:.5f} | max: {:.5f}'.format(
# elapsed_time(start_time), max_distance, nearby_tolerance))
set_joint_positions(self.robot, self.arm_joints, conf)
return get_configuration(self.robot)
#########################
def set_initial_conf(self):
set_joint_positions(self.robot, self.base_joints, [2.0, 0, np.pi])
#for rule in self.robot_yaml['cspace_to_urdf_rules']: # gripper: max is open
# joint = joint_from_name(self.robot, rule['name'])
# set_joint_position(self.robot, joint, rule['value'])
set_joint_positions(self.robot, self.arm_joints,
self.default_conf) # active_task_spaces
# if self.robot_name == EVE:
# for arm in ARMS:
# joints = get_eve_arm_joints(self.robot, arm)[2:4]
# set_joint_positions(self.robot, joints, -0.2*np.ones(len(joints)))
def set_gripper(self, value):
positions = value * np.ones(len(self.gripper_joints))
set_joint_positions(self.robot, self.gripper_joints, positions)
def close_gripper(self):
self.closed_gq.assign()
def open_gripper(self):
self.open_gq.assign()
#########################
def get_door_sign(self, joint):
return -1 if 'left' in get_joint_name(self.kitchen, joint) else +1
def closed_conf(self, joint):
lower, upper = get_joint_limits(self.kitchen, joint)
if 'drawer' in get_joint_name(self.kitchen, joint):
fraction = 0.9
return fraction * lower + (1 - fraction) * upper
if 'left' in get_joint_name(self.kitchen, joint):
return upper
return lower
def open_conf(self, joint):
joint_name = get_joint_name(self.kitchen, joint)
if 'left' in joint_name:
open_position = get_min_limit(self.kitchen, joint)
else:
open_position = get_max_limit(self.kitchen, joint)
#print(get_joint_name(self.kitchen, joint), get_min_limit(self.kitchen, joint), get_max_limit(self.kitchen, joint))
# drawers: [0.0, 0.4]
# left doors: [-1.57, 0.0]
# right doors: [0.0, 1.57]
if joint_name in CABINET_JOINTS:
# TODO: could make fraction of max
return CABINET_OPEN_ANGLE * open_position / abs(open_position)
if joint_name in DRAWER_JOINTS:
return DRAWER_OPEN_FRACTION * open_position
return open_position
def close_door(self, joint):
set_joint_position(self.kitchen, joint, self.closed_conf(joint))
def open_door(self, joint):
set_joint_position(self.kitchen, joint, self.open_conf(joint))
#########################
def add_camera(self,
name,
pose,
camera_matrix,
max_depth=KINECT_DEPTH,
display=False):
color = apply_alpha(RED, 0.1 if DEBUG else 0)
cone = get_viewcone(depth=max_depth,
camera_matrix=camera_matrix,
color=color,
mass=0,
collision=False)
set_pose(cone, pose)
if display:
kinect = load_pybullet(KINECT_URDF, fixed_base=True)
set_pose(kinect, pose)
set_color(kinect, BLACK)
self.add(name, kinect)
self.cameras[name] = Camera(cone, camera_matrix, max_depth)
if DEBUG:
draw_pose(pose)
step_simulation()
return name
def get_supporting(self, obj_name):
# is_placed_on_aabb | is_center_on_aabb
# Only want to generate stable placements, but can operate on initially unstable ones
# TODO: could filter orientation as well
body = self.get_body(obj_name)
supporting = {
surface
for surface in ALL_SURFACES
if is_center_on_aabb(body,
compute_surface_aabb(self, surface),
above_epsilon=5e-2,
below_epsilon=5e-2)
}
if ('range' in supporting) and (len(supporting) == 2):
# TODO: small hack for now
supporting -= {'range'}
if len(supporting) != 1:
print('{} is not supported by a single surface ({})!'.format(
obj_name, supporting))
return None
[surface_name] = supporting
return surface_name
def fix_pose(self, name, pose=None, fraction=0.5):
body = self.get_body(name)
if pose is None:
pose = get_pose(body)
else:
set_pose(body, pose)
# TODO: can also drop in simulation
x, y, z = point_from_pose(pose)
roll, pitch, yaw = euler_from_quat(quat_from_pose(pose))
quat = quat_from_euler(Euler(yaw=yaw))
set_quat(body, quat)
surface_name = self.get_supporting(name)
if surface_name is None:
return None, None
if fraction == 0:
new_pose = (Point(x, y, z), quat)
return new_pose, surface_name
surface_aabb = compute_surface_aabb(self, surface_name)
new_z = (1 - fraction) * z + fraction * stable_z_on_aabb(
body, surface_aabb)
point = Point(x, y, new_z)
set_point(body, point)
print('{} error: roll={:.3f}, pitch={:.3f}, z-delta: {:.3f}'.format(
name, roll, pitch, new_z - z))
new_pose = (point, quat)
return new_pose, surface_name
# def fix_geometry(self):
# for name in self.movable:
# fixed_pose, _ = self.fix_pose(name)
# if fixed_pose is not None:
# set_pose(self.get_body(name), fixed_pose)
#########################
def add(self, name, body):
assert name not in self.body_from_name
if DEBUG:
add_body_name(body, name)
self.body_from_name[name] = body
return name
def add_body(self, name, **kwargs):
obj_type = type_from_name(name)
self.names_from_type.setdefault(obj_type, []).append(name)
path = get_obj_path(obj_type)
#self.path_from_name[name] = path
print('Loading', path)
body = load_pybullet(path, **kwargs)
assert body is not None
self.add(name, body)
def get_body(self, name):
return self.body_from_name[name]
# def get_body_path(self, name):
# return self.path_from_name[name]
# def get_body_type(self, name):
# filename, _ = os.path.splitext(os.path.basename(self.get_body_path(name)))
# return filename
def get_name(self, name):
inverse = {v: k for k, v in self.body_from_name.items()}
return inverse.get(name, None)
def remove_body(self, name):
body = self.get_body(name)
remove_body(body)
del self.body_from_name[name]
def reset(self):
#remove_all_debug()
for camera in self.cameras.values():
remove_body(camera.body)
#remove_body(camera.kinect)
self.cameras = {}
for name in list(self.body_from_name):
self.remove_body(name)
def destroy(self):
reset_simulation()
disconnect()
class Arm(object):
""" UR3 arm controller """
def __init__(self,
ft_sensor=False,
driver=ROBOT_GAZEBO,
ee_transform=None,
robot_urdf='ur3e_robot',
ik_solver=IKFAST,
namespace='ur3',
gripper=False,
ft300=False):
""" ft_sensor bool: whether or not to try to load ft sensor information
driver string: type of driver to use for real robot or simulation.
supported: gazebo, ur_modern_driver, ur_rtde_driver (Newest)
use ur_control.constants e.g. ROBOT_GAZEBO
ee_tranform array [x,y,z,ax,ay,az,w]: optional transformation to the end-effector
that is applied before doing any operation in task-space
robot_urdf string: name of the robot urdf file to be used
namespace string: nodes namespace prefix
gripper bool: enable gripper control
"""
cprint.ok("ft_sensor: {}, driver: {}, ee_transform: {}, \n robot_urdf: {}".format(ft_sensor, driver, ee_transform, robot_urdf))
self._joint_angle = dict()
self._joint_velocity = dict()
self._joint_effort = dict()
self._current_ft = []
self.ft_sensor = None
self.ft300 = ft300
self.ik_solver = ik_solver
self.ee_transform = ee_transform
self.ns = namespace
self.ee_link = 'tool0'
self.max_joint_speed = np.deg2rad([100, 100, 100, 200, 200, 200])
# self.max_joint_speed = np.deg2rad([191, 191, 191, 371, 371, 371])
self._init_ik_solver(robot_urdf)
self._init_controllers(driver, gripper)
if ft_sensor:
self._init_ft_sensor(driver)
### private methods ###
def _init_controllers(self, driver, gripper):
traj_publisher = None
namespace = ''
if driver == ROBOT_UR_MODERN_DRIVER:
traj_publisher = JOINT_PUBLISHER_REAL
elif driver == ROBOT_UR_RTDE_DRIVER:
traj_publisher = JOINT_PUBLISHER_BETA
elif driver == ROBOT_GAZEBO:
traj_publisher = JOINT_PUBLISHER_SIM
elif driver == ROBOT_GAZEBO_DUAL_LEFT:
traj_publisher = JOINT_PUBLISHER_SIM
namespace = JOINT_PUBLISHER_SIM_DUAL_LEFT
elif driver == ROBOT_GAZEBO_DUAL_RIGHT:
traj_publisher = JOINT_PUBLISHER_SIM
namespace = JOINT_PUBLISHER_SIM_DUAL_RIGHT
else:
raise Exception("unsupported driver", driver)
# Flexible trajectory (point by point)
traj_publisher_flex = '/' + namespace + traj_publisher + '/command'
print(traj_publisher_flex)
cprint.blue("connecting to: {}".format(traj_publisher))
self._flex_trajectory_pub = rospy.Publisher(traj_publisher_flex,
JointTrajectory,
queue_size=10)
self.joint_traj_controller = JointTrajectoryController(
publisher_name=traj_publisher, namespace=namespace)
self.gripper = None
if gripper:
self.gripper = GripperController(namespace=namespace, timeout=2.0)
def _init_ik_solver(self, robot_urdf):
self.kdl = ur_kinematics(robot_urdf, ee_link=self.ee_link)
if self.ik_solver == IKFAST:
# IKfast libraries
if robot_urdf == 'ur3_robot':
self.arm_ikfast = ur_ikfast.URKinematics('ur3')
elif robot_urdf == 'ur3e_robot':
self.arm_ikfast = ur_ikfast.URKinematics('ur3e')
elif self.ik_solver == TRAC_IK:
self.trac_ik = IK(base_link="base_link", tip_link=self.ee_link,
timeout=0.001, epsilon=1e-5, solve_type="Speed",
urdf_string=utils.load_urdf_string('ur_pykdl', robot_urdf))
else:
raise Exception("unsupported ik_solver", self.ik_solver)
def _init_ft_sensor(self, driver):
# Publisher of wrench
self.pub_ee_wrench = rospy.Publisher('/%s/ee_ft' % self.ns,
Wrench,
queue_size=50)
self.pub_raw_ft = rospy.Publisher('/%s/filtered_ft' % self.ns,
Wrench,
queue_size=50)
if driver == ROBOT_GAZEBO:
self.ft_sensor = FTsensor(namespace=FT_SUBSCRIBER_SIM)
else:
if self.ft300:
self.ft_sensor = FTsensor(namespace=FT300_SUBSCRIBER_REAL)
else:
self.ft_sensor = FTsensor(namespace=FT_SUBSCRIBER_REAL)
rospy.sleep(1)
self.set_wrench_offset(override=False)
def _update_wrench_offset(self):
self.wrench_offset = self.get_filtered_ft().tolist()
rospy.set_param('/%s/ft_offset' % self.ns, self.wrench_offset)
def _flexible_trajectory(self, position, time=5.0, vel=None):
""" Publish point by point making it more flexible for real-time control """
# Set up a trajectory message to publish.
action_msg = JointTrajectory()
action_msg.joint_names = JOINT_ORDER
# Create a point to tell the robot to move to.
target = JointTrajectoryPoint()
target.positions = position
# These times determine the speed at which the robot moves:
if vel is not None:
target.velocities = [vel] * 6
target.time_from_start = rospy.Duration(time)
# Package the single point into a trajectory of points with length 1.
action_msg.points = [target]
self._flex_trajectory_pub.publish(action_msg)
def _solve_ik(self, pose):
if self.ee_transform is not None:
inv_ee_transform = np.copy(self.ee_transform)
inv_ee_transform[:3] *= -1
pose = np.array(conversions.transform_end_effector(pose, inv_ee_transform))
if self.ik_solver == IKFAST:
ik = self.arm_ikfast.inverse(pose, q_guess=self.joint_angles())
elif self.ik_solver == TRAC_IK:
ik = self.trac_ik.get_ik(self.joint_angles(), pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], pose[6])
if ik is None:
print("IK not found")
return ik
### Public methods ###
def get_filtered_ft(self):
""" Get measurements from FT Sensor.
Measurements are filtered with a low-pass filter.
Measurements are given in sensors orientation.
"""
if self.ft_sensor is None:
raise Exception("FT Sensor not initialized")
ft_limitter = [300, 300, 300, 30, 30, 30] # Enforce measurement limits (simulation)
ft = self.ft_sensor.get_filtered_wrench()
ft = [
ft[i] if abs(ft[i]) < ft_limitter[i] else ft_limitter[i]
for i in range(6)
]
return np.array(ft)
def set_wrench_offset(self, override=False):
""" Set a wrench offset """
if override:
self._update_wrench_offset()
else:
self.wrench_offset = rospy.get_param('/%s/ft_offset' % self.ns, None)
if self.wrench_offset is None:
self._update_wrench_offset()
def get_ee_wrench_hist(self, hist_size=24):
if self.ft_sensor is None:
raise Exception("FT Sensor not initialized")
q_hist = self.joint_traj_controller.get_joint_positions_hist()[:hist_size]
ft_hist = self.ft_sensor.get_filtered_wrench(hist_size=hist_size)
if self.wrench_offset is not None:
ft_hist = np.array(ft_hist) - np.array(
self.wrench_offset)
poses_hist = [self.end_effector(q) for q in q_hist]
wrench_hist = [spalg.convert_wrench(wft, p).tolist() for p, wft in zip(poses_hist, ft_hist)]
return np.array(wrench_hist)
def get_ee_wrench(self):
""" Compute the wrench (force/torque) in task-space """
if self.ft_sensor is None:
return np.zeros(6)
wrench_force = self.ft_sensor.get_filtered_wrench()
if self.wrench_offset is not None:
wrench_force = np.array(wrench_force) - np.array(
self.wrench_offset)
# compute force transformation?
# # # Transform of EE
pose = self.end_effector()
return spalg.convert_wrench(wrench_force, pose)
def publish_wrench(self):
if self.ft_sensor is None:
raise Exception("FT Sensor not initialized")
" Publish arm's end-effector wrench "
wrench = self.get_ee_wrench()
# Note you need to call rospy.init_node() before this will work
self.pub_ee_wrench.publish(conversions.to_wrench(wrench))
def publish_ft_raw(self):
if self.ft_sensor is None:
raise Exception("FT Sensor not initialized")
" Publish arm's end-effector wrench "
wrench_force = self.ft_sensor.get_filtered_wrench()
if self.wrench_offset is not None:
wrench_force = np.array(wrench_force) - np.array(
self.wrench_offset)
# Note you need to call rospy.init_node() before this will work
self.pub_raw_ft.publish(conversions.to_wrench(wrench_force))
def end_effector(self,
joint_angles=None,
rot_type='quaternion'):
""" Return End Effector Pose """
joint_angles = self.joint_angles() if joint_angles is None else joint_angles
if rot_type == 'quaternion':
# forward kinematics
if self.ik_solver == IKFAST:
x = self.arm_ikfast.forward(joint_angles)
else:
x = self.kdl.forward_position_kinematics(joint_angles)
# apply extra transformation of end-effector
if self.ee_transform is not None:
x = np.array(conversions.transform_end_effector(x, self.ee_transform))
return x
elif rot_type == 'euler':
x = self.end_effector(joint_angles)
euler = np.array(transformations.euler_from_quaternion(x[3:], axes='rxyz'))
return np.concatenate((x[:3], euler))
else:
raise Exception("Rotation Type not supported", rot_type)
def joint_angle(self, joint):
"""
Return the requested joint angle.
@type joint: str
@param joint: name of a joint
@rtype: float
@return: angle in radians of individual joint
"""
return self.joint_traj_controller.get_joint_positions()[joint]
def joint_angles(self):
"""
Return all joint angles.
@rtype: dict({str:float})
@return: unordered dict of joint name Keys to angle (rad) Values
"""
return self.joint_traj_controller.get_joint_positions()
def joint_velocity(self, joint):
"""
Return the requested joint velocity.
@type joint: str
@param joint: name of a joint
@rtype: float
@return: velocity in radians/s of individual joint
"""
return self.joint_traj_controller.get_joint_velocities()[joint]
def joint_velocities(self):
"""
Return all joint velocities.
@rtype: dict({str:float})
@return: unordered dict of joint name Keys to velocity (rad/s) Values
"""
return self.joint_traj_controller.get_joint_velocities()
### Basic Control Methods ###
def set_joint_positions(self,
position,
velocities=None,
accelerations=None,
wait=False,
t=5.0):
self.joint_traj_controller.add_point(positions=position,
time=t,
velocities=velocities,
accelerations=accelerations)
self.joint_traj_controller.start(delay=0.01, wait=wait)
self.joint_traj_controller.clear_points()
def set_joint_positions_flex(self, position, t=5.0, v=None):
qc = self.joint_angles()
deltaq = (qc - position)
speed = deltaq / t
cmd = position
if np.any(np.abs(speed) > (self.max_joint_speed/t)):
print("exceeded max speed", speed)
return SPEED_LIMIT_EXCEEDED
self._flexible_trajectory(cmd, t, v)
return DONE
def set_target_pose(self, pose, wait=False, t=5.0):
""" Supported pose is only x y z aw ax ay az """
q = self._solve_ik(pose)
if q is None:
# IK not found
return IK_NOT_FOUND
else:
return self.set_joint_positions(q, wait=wait, t=t)
def set_target_pose_flex(self, pose, t=5.0):
""" Supported pose is only x y z aw ax ay az """
q = self._solve_ik(pose)
if q is None:
# IK not found
return IK_NOT_FOUND
else:
return self.set_joint_positions_flex(q, t=t)
