def set_static_intent(self, intent=None):
if self.sub_intent is not None:
self.sub_intent.unregister()
self.sub_intent = None
if intent is not None:
self.intent = colvec(intent)
def set_static_configuration_ref(self, configuration_ref=None):
if self.sub_config is not None:
self.sub_config.unregister()
self.sub_config = None
if configuration_ref is not None:
self.configuration_ref = colvec(configuration_ref)
def __init__(self,
frame_tracker,
configuration_ref,
k_conf=1.0,
k_grip=0.1,
box_joints_topic=None,
forward_kinematics=None):
"""
Class which computes kinematics on an object relative to the robots grip on it and a desired configuration
:param get_jacobian: a function handle which returns the current objects jacobian, takes no arguments
:param configuration_ref: the initial reference configuration, this can be updated dynamically
:param forward_kinematics: a function handle which computes the forward kinematics from a joint array
"""
if box_joints_topic is None:
box_joints_topic = 'box/joint_states'
self.k_conf = k_conf
self.k_grip = k_grip
# copy across member variables
self._frame_tracker = frame_tracker
self.get_jacobian = self._frame_tracker.compute_jacobian_matrix
self.configuration_ref = colvec(configuration_ref)
self.forward_kinematics = forward_kinematics
# block until we get out first joint update
self.joints = colvec(
rospy.wait_for_message(box_joints_topic, JointState).position)
# set up a subscriber to this joint topic
self.sub_joints = rospy.Subscriber(box_joints_topic, JointState,
self._update_joints)
# placeholder for a reference subscriber
self.sub_config = None
self._tf_listener = tf.TransformListener()
self.optimal_grip_orientation = self.get_grip_transform()
rospy.logdebug('ObjectConfigKinematics init complete')
def compute_grip_error(self):
obj_grip_error = self.optimal_grip_orientation - self.get_grip_transform(
)
obj_grip_error[1] = 0
base_grip_error = self._frame_tracker.compute_transform(
'base', 'trans3').R().dot(colvec(obj_grip_error))[:, 0]
grip_diff = np.zeros(6)
grip_diff[3:] = base_grip_error
return grip_diff
def compute_optimal_robot_effector_velocity(self):
"""
Computes the optimal velocity end effector in order to close the error from the configuration reference
:return:
"""
config_error = self.configuration_ref - self.joints
# Perform this before grip correction so that appropriate tf frames are published by the get_jacobian call
config_correction = colvec(self.get_jacobian().dot(-self.k_conf *
(config_error)))
grip_error = self.compute_grip_error()
grip_correction = self.k_grip * colvec(grip_error)
rospy.logdebug('\nconfig error : ' + str(config_error.flatten()) +
'\ngrip error : ' +
str(grip_error.flatten()[[3, 5]]) +
'\nconfig correction: ' +
str(config_correction.flatten()) +
'\ngrip correction : ' +
str(grip_correction.flatten()))
return config_correction + grip_correction
def __init__(self, num_joints, get_object_human_jacobian, get_object_robot_jacobian, joint_angles_topic,
configuration_ref, intent, initial_alpha=None, get_initial_guess=None, k_p=1.0):
self.num_joints = num_joints
self.get_object_human_jacobian = get_object_human_jacobian
self.get_object_robot_jacobian = get_object_robot_jacobian
self.configuration_ref = colvec(configuration_ref)
self.intent = colvec(intent)
self.alpha = np.array(initial_alpha) if initial_alpha is not None else self.default_alpha.copy()
self.k_p = k_p
self.sub_config = None
self.sub_intent = None
self.pub_costs = rospy.Publisher('~costs', Float32MultiArray, queue_size=100)
if get_initial_guess is None:
self.get_initial_guess = self._get_config_ref
else:
self.get_initial_guess = get_initial_guess
# block until we get out first joint update
self.joints = colvec(rospy.wait_for_message(joint_angles_topic, JointState).position)
# set up a subscriber to this joint topic
self.sub_joints = rospy.Subscriber(joint_angles_topic, JointState, self._update_joints)
def direct_full_pose(self, pose_vel, neutral_error):
"""
Directly computes the cascaded pose scenario by directly applying the analytical solution specific to this case
TODO: The general approach needs to be debugged so that its output for such a setup mimics this output
:param pose_vel: the reference velocity of the pose
:param secondary: flag to switch on or off rotational control
:return: the joint angles for the arm
"""
# an int which indicates how many of the desired tasks are feasible (between 0 and 2)
feasibility = 0
# initialize the velocity vector
dtheta = np.zeros((7, 1))
# make sure pose velocity reference is a column vector
dx1 = np.array(pose_vel).reshape((-1, 1))
# get the positional Jacobian
J1 = np.array(self.get_full_jacobian())
# if it is not full rank then do not invert and just return the current dtheta command and the feasibility
if npla.matrix_rank(J1, tol=0.1) != 6:
rospy.logdebug("Feasability: %d" % feasibility)
return dtheta, feasibility
# if we got here, the first task is feasible, so increment feasibility
feasibility = 1
# get the inverse of the positional Jacobian
J1p = npla.pinv(J1)
# add the joint angles which minimize error from this reference
dtheta += J1p.dot(dx1)
N1 = self.null(J1)
# project neutral configuration onto the nullspace of pose task
dtheta2 = multidot(N1, colvec(neutral_error))
# add the joint angles which minimize error of the second task within the nullspace of the first task's solution
dtheta += dtheta2
# return the joint angles and feasibility
rospy.logdebug("Feasability: %d" % feasibility)
return dtheta, feasibility
def __init__(self, limb, rate, cascaded=False):
self.cascaded = cascaded
self._neutral_config = NEUTRAL[limb]
self._kin = baxter_kinematics(limb)
self._limb = baxter_interface.Limb(limb)
self._nullspace_projector = TaskHeirarchy(7, rate)
rospy.logdebug(limb)
self._nullspace_projector.setup_cascaded_pose_vel(
np.zeros(7), self._kin.jacobian)
rospy.logdebug(self._nullspace_projector.get_full_jacobian())
# pretend our initial state was moved to from neutral state
joint_velocity_command = self.get_joint_array() - NEUTRAL_ARRAY
# Clamp the joint velocity command so it's within limits for each joint
joint_vel_scalar = max(abs(
joint_velocity_command.flatten())) / (MAX_JOINT_VEL * 0.1)
if joint_vel_scalar > 1:
joint_velocity_command /= joint_vel_scalar
# set this command as the last one so if we start in a singularity we get out by
# moving closer to neutral
self._last_velocity_cmd = colvec(joint_velocity_command)
def _update_joints(self, msg):
# copy across angles into a column vector
self.joints = colvec(msg.position)
def _update_configuration_ref(self, msg):
# copy across angles into a column vector
self.configuration_ref = colvec(msg.position)
def _update_intent(self, msg):
self.intent = colvec([msg.linear.x, msg.linear.y, msg.linear.z])
def _update_configuration_ref(self, msg):
self.configuration_ref = colvec(msg.position)
def effector_movement_cost(self, q):
return npla.norm(self.get_object_robot_jacobian(np.squeeze(self.joints)).dot(colvec(q) - self.joints))
def configuration_cost(self, q):
return npla.norm(colvec(q)-self.configuration_ref)
def compute_optimal_joint_anlges(self, initial_guess=None):
if initial_guess is None:
initial_guess = self.get_initial_guess()
return colvec(minimize(self.total_cost, initial_guess).x)
