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 __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 __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 __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 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
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 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
def __init__(self):
self.robot = URDF.from_parameter_server()
self.kdl_kin = KDLKinematics(self.robot, "base_link", "end1")
self.end_pos_e = np.array([0, 0, 0, 1])
self.end_pos_e = self.end_pos_e.reshape(self.end_pos_e.shape[0], 1)
self.ik_solver = IK("base_link", "end1")
self.seed_state = [0.0] * self.ik_solver.number_of_joints
def __init__(self, urdf_str):
self.urdf_tree = URDF.from_xml_string(urdf_str)
base_link = "link0"
nlinks = len(self.urdf_tree.link_map.keys())
end_link = "link{}".format(nlinks - 1)
print("Link chain goes from {} to {}".format(base_link, end_link))
self.kdl_kin = KDLKinematics(self.urdf_tree, base_link, end_link)
self.ik_solver = IK(base_link, end_link, urdf_string=urdf_str)
def __init__(self):
# 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 __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 __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 __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()
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 __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 __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 _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__(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
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()
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
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 __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 __init__(self, hebiros_wrapper, urdf_str, base_link, end_link):
"""SMACH State
:type hebiros_wrapper: HebirosWrapper
:param hebiros_wrapper: HebirosWrapper instance for Leg HEBI group
:type urdf_str: str
:param urdf_str: Serialized URDF str
:type base_link: str
:param base_link: Leg base link name in URDF
:type end_link: str
:param end_link: Leg end link name in URDF
"""
State.__init__(self,
outcomes=['ik_failed', 'success'],
input_keys=[
'prev_joint_pos', 'target_end_link_point',
'execution_time'
],
output_keys=['prev_joint_pos', 'active_joints'])
self.hebi_wrap = hebiros_wrapper
self.urdf_str = urdf_str
self.base_link = base_link
self.end_link = end_link
self.active = False
# hardware interface
self._hold_leg_position = True
self._hold_joint_angles = []
self.hebi_wrap.add_feedback_callback(self._hold_leg_pos_cb)
# pykdl
self.kdl_fk = KDLKinematics(URDF.from_xml_string(urdf_str), base_link,
end_link)
self._active_joints = self.kdl_fk.get_joint_names()
# trac-ik
self.trac_ik = IK(base_link,
end_link,
urdf_string=urdf_str,
timeout=0.01,
epsilon=1e-4,
solve_type="Distance")
self.ik_pos_xyz_bounds = [0.01, 0.01, 0.01]
self.ik_pos_wxyz_bounds = [31416.0, 31416.0, 31416.0
] # NOTE: This implements position-only IK
# joint state publisher
while not rospy.is_shutdown() and len(
self.hebi_wrap.get_joint_positions()) < len(
self.hebi_wrap.hebi_mapping):
rospy.sleep(0.1)
self._joint_state_pub = rospy.Publisher('joint_states',
JointState,
queue_size=1)
self.hebi_wrap.add_feedback_callback(self._joint_state_cb)
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
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
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 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 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 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 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()
