class Baxter(object):
"""
Iinterface for controlling the real and simulated
Baxter robot.
"""
def __init__(self,
sim=False,
config=None,
time_step=1.0,
rate=100.0,
missed_cmds=20000.0):
"""
Initialize Baxter Interface
Args:
sim (bool): True specifies using the PyBullet simulator
False specifies using the real robot
arm (str): 'right' or 'left'
control (int): Specifies control type
TODO: change to str ['ee', 'position', 'velocity']
config (class): Specifies joint ranges, ik null space paramaters, etc
time_step (float):
TODO: probably get rid of this
rate (float):
missed_cmds (float):
"""
self.sim = sim
self.dof = {
'left': {
'ee': 6,
'position': 7,
'velocity': 7
},
'right': {
'ee': 6,
'position': 7,
'velocity': 7
},
'both': {
'ee': 12,
'position': 14,
'velocity': 14
}
}
if self.sim:
import pybullet as p
import baxter_pybullet_interface as pybullet_interface
self.baxter_path = "assets/baxter_robot/baxter_description/urdf/baxter.urdf"
self.time_step = time_step
else:
import rospy
from baxter_pykdl import baxter_kinematics
import baxter_interface
from baxter_interface import CHECK_VERSION, Limb, Gripper
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
from std_msgs.msg import Header
from baxter_core_msgs.srv import SolvePositionIK, SolvePositionIKRequest
self.rate = rate
self.freq = 1 / rate
self.missed_cmds = missed_cmds
self.config = config
self.initial_setup()
def set_command_time_out(self):
"""
Set command timeout for sending ROS commands
"""
if self.sim:
pass
else:
self.left_arm.set_command_timeout(self.freq * self.missed_cmds)
self.right_arm.set_command_timeout(self.freq * self.missed_cmds)
return
def initial_setup(self):
if self.sim:
# load baxter
objects = [p.loadURDF(self.baxter_path, useFixedBase=True)]
self.baxter_id = objects[0]
print("baxter_id: ", self.baxter_id)
if self.config:
self.use_nullspace = True
self.use_orientation = True
self.ll = self.config.nullspace.ll
self.ul = self.config.nullspace.ul
self.jr = self.config.nullspace.jr
self.rp = self.config.nullspace.rp
else:
self.use_nullspace = False
self.create_arms()
else:
self.create_arms()
self.set_command_time_out()
self.control_rate = rospy.Rate(self.rate)
def reset(self, initial_pose=None):
if self.sim:
self._reset_sim(initial_pose)
else:
self._reset_real(initial_pose)
def _reset_real(self, initial_pose=None):
self.enable()
time.sleep(0.5)
self.set_initial_position('right')
self.set_initial_position('left', blocking=True)
self.calibrate_grippers()
def _reset_sim(self, control_type=None, initial_pose=None):
# set control type
if control_type == 'position' or control_type == 'ee' or control_type is None:
control_mode = p.POSITION_CONTROL
elif control_type == 'velocity':
control_mode = p.VELOCITY_CONTROL
elif control_mode == 'torque':
control_mode = p.TORQUE_CONTROL
else:
raise ValueError(
"control_type must be in ['position', 'ee', 'velocity', 'torque']."
)
# set initial position
if initial_pose:
for joint_index, joint_val in initial_pose:
p.resetJointState(0, joint_index, joint_val)
p.setJointMotorControl2(self.baxter_id,
jointIndex=joint_index,
controlMode=control_mode,
targetPosition=joint_val)
else:
num_joints = p.getNumJoints(self.baxter_id)
for joint_index in range(num_joints):
p.setJointMotorControl2(self.baxter_id,
joint_index,
control_mode,
maxForce=self.config.max_force)
return
def create_arms(self):
"""
Create arm interface objects for Baxter.
An arm consists of a Limb and its Gripper.
"""
# create arm objects
if self.sim:
self.left_arm = pybullet_interface.Limb(self.baxter_id, "left",
self.config)
# self.left_arm.gripper = pybullet_interface.Gripper("left")
self.right_arm = pybullet_interface.Limb(self.baxter_id, "right",
self.config)
# self.right_arm.gripper = pybullet_interface.Gripper("right")
else:
self.left_arm = Limb("left")
self.left_arm.gripper = Gripper("left")
self.left_arm.kin = baxter_kinematics("left")
self.right_arm = Limb("right")
self.right_arm.gripper = Gripper("right")
self.right_arm.kin = baxter_kinematics("right")
return
def calibrate_grippers(self):
"""
(Blocking) Calibrates gripper(s) if not
yet calibrated
"""
if not self.left_arm.gripper.calibrated():
self.left_arm.gripper.calibrate()
if not self.right_arm.gripper.calibrated():
self.right_arm.gripper.calibrate()
return
def enable(self):
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
self._rs.enable()
def shutdown(self):
pass
def set_joint_velocities(self, arm, joint_velocities):
"""
Set joint velocites for a given arm
Args
arm (str): 'left' or 'right' or 'both'
joint_velocities (list or tuple or numpy array):
Array of len 7 or 14
"""
if arm == 'left':
action_dict = self.create_action_dict('left', 'velocity',
joint_velocities)
self.left_arm.set_joint_velocities(action_dict)
elif arm == 'right':
action_dict = self.create_action_dict('right', 'velocity',
joint_velocities)
self.right_arm.set_joint_velocities(action_dict)
elif arm == 'both':
l_velocities = joint_velocities[:7]
r_velocities = joint_velocities[7:]
l_action_dict = self.create_action_dict('left', 'velocity',
l_velocities)
r_action_dict = self.create_action_dict('right', 'velocity',
r_velocities)
self.left_arm.set_joint_velocities(l_action_dict)
self.right_arm.set_joint_velocities(r_action_dict)
return
def move_to_joint_positions(self, arm, joint_positions):
"""
Move specified arm to give joint positions
(This function is blocking.)
Args
arm (str): 'left' or 'right' or 'both'
joint_positionns (list or tuple or numpy array):
Array of len 7 or 14
"""
if arm == 'left':
action_dict = self.create_action_dict('left', 'position',
joint_positions)
self.left_arm.move_to_joint_positions(action_dict)
elif arm == 'right':
action_dict = self.create_action_dict('right', 'position',
joint_positions)
self.right_arm.move_to_joint_positions(action_dict)
elif arm == 'both':
l_joints = joint_positions[:7]
r_joints = joint_positions[7:]
l_action_dict = self.create_action_dict('left', 'position',
l_joints)
r_action_dict = self.create_action_dict('right', 'position',
r_joints)
self.left_arm.move_to_joint_positions(l_action_dict)
self.right_arm.move_to_joint_positions(r_action_dict)
return
def set_joint_positions(self, arm, joint_positions):
"""
Move specified arm to give joint positions
(This function is not blocking.)
Args
arm (str): 'left' or 'right' or 'both'
joint_positionns (list or tuple or numpy array):
Array of len 7 or 14
"""
if arm == 'left':
action_dict = self.create_action_dict('left', 'position',
joint_positions)
self.left_arm.set_joint_positions(action_dict)
elif arm == 'right':
action_dict = self.create_action_dict('right', 'position',
joint_positions)
self.right_arm.set_joint_positions(action_dict)
elif arm == 'both':
l_joints = joint_positions[:7]
r_joints = joint_positions[7:]
l_action_dict = self.create_action_dict('left', 'position',
l_joints)
r_action_dict = self.create_action_dict('right', 'position',
r_joints)
self.left_arm.set_joint_positions(l_action_dict)
self.right_arm.set_joint_positions(r_action_dict)
return
def move_to_ee_pose(self, arm, pose, blocking=True):
"""
Move end effector to specified pose
Args
arm (string): "left" or "right" or "both"
pose (list):
if arm == 'left' or arm == 'right':
pose = [X, Y, Z, r, p, w]
else:
pose = left_pose + right _pose
"""
if arm == "both":
left_pose = pose[:6]
right_pose = pose[6:]
left_joints = self.ik('left', left_pose)
right_joints = self.ik('right', right_pose)
if blocking:
self.left_arm.move_to_joint_positions(left_joints)
self.right_arm.move_to_joint_positions(right_joints)
else:
self.left_arm.set_joint_positions(left_joints)
self.right_arm.set_joint_positions(right_joints)
elif arm == "left":
joints = self.ik(arm, pose)
if blocking:
self.left_arm.move_to_joint_positions(joints)
else:
self.left_arm.set_joint_positions(joints)
elif arm == "right":
joints = self.ik(arm, pose)
if blocking:
self.right_arm.move_to_joint_positions(joints)
else:
self.right_arm.set_joint_positions(joints)
else:
raise ValueError("Arm must be 'right', 'left', or 'both'")
return
def get_ee_pose(self, arm, mode=None):
"""
Returns end effector pose for specified arm.
End effector pose is a list of values corresponding to the 3D cartesion coordinates
and roll (r), pitch (p), and yaw (w) Euler angles.
Args
arm (string): "right" or "left"
mode (string): "Quaternion" or "quaterion" or "quat"
Returns
pose (list): Euler angles [X,Y,Z,r,p,w] or Quaternion [X,Y,Z,x,y,z,w]
"""
if arm == 'left' or arm == 'right':
pos = self.get_ee_position(arm)
orn = self.get_ee_orientation(arm, mode)
return pos + orn
elif arm == 'both':
l_pos = self.get_ee_position('left')
l_orn = self.get_ee_orientation('left', mode)
r_pos = self.get_ee_position('right')
r_orn = self.get_ee_orientation('right', mode)
return l_pos + l_orn + r_pos + r_orn
def get_ee_position(self, arm):
"""
Returns end effector position for specified arm.
Returns the 3D cartesion coordinates of the end effector.
Args
arm (string): "left" or "right" or "both"
Returns
[X,Y,Z]
"""
if arm not in ['left', 'right', 'both']:
raise ValueError("Arg arm should be 'left' or 'right' or 'both'.")
if arm == "left":
return list(self.left_arm.endpoint_pose()['position'])
elif arm == "right":
return list(self.right_arm.endpoint_pose()['position'])
elif arm == "both":
return list(self.left_arm.endpoint_pose()['position']) + list(
self.right_arm.endpoint_pose()['position'])
def get_ee_orientation(self, arm, mode=None):
"""
Returns a list of the orientations of the end effectors(s)
Args
arm (string): "right" or "left" or "both"
mode (string): specifies angle representation
Returns
orn (list): list of Euler angles or Quaternion
"""
if arm not in ['left', 'right', 'both']:
raise ValueError("Arg arm should be 'left' or 'right'.")
if arm == "left":
orn = list(self.left_arm.endpoint_pose()['orientation'])
elif arm == "right":
orn = list(self.right_arm.endpoint_pose()['orientation'])
elif arm == "both":
orn = list(self.left_arm.endpoint_pose()['orientation']) + list(
self.right_arm.endpoint_pose()['orientation'])
return list(p.getEulerFromQuaternion(orn))
def get_joint_angles(self, arm):
"""
Get joint angles for specified arm
Args
arm(strin): "right" or "left" or "both"
Returns
joint_angles (list): List of joint angles starting from the right_s0 ('right_upper_shoulder')
and going down the kinematic tree to the end effector.
"""
if arm == "left":
joint_angles = self.left_arm.joint_angles()
elif arm == "right":
joint_angles = self.right_arm.joint_angles()
elif arm == "both":
joint_angles = self.left_arm.joint_angles(
) + self.right_arm.joint_angles()
else:
raise ValueError("Arg arm should be 'left' or 'right' or 'both'.")
if not self.sim:
joint_angles = joint_angles.values()
return joint_angles
def get_joint_velocities(self, arm):
"""
Get joint velocites for specified arm
"""
if arm == "left":
return self.left_arm.joint_velocities()
elif arm == "right":
return self.right_arm.joint_velocities()
elif arm == "both":
return self.left_arm.joint_velocities(
) + self.right_arm.joint_velocities()
else:
raise ValueError("Arg arm should be 'left' or 'right' or 'both'.")
if not self.sim:
joint_velocities = joint_velocities.values()
return joint_velocities
def get_joint_efforts(self, arm):
"""
Get joint torques for specified arm
"""
if arm == "left":
return self.left_arm.joint_effort()
elif arm == "right":
return self.right_arm.joint_efforts()
elif arm == "both":
return self.left_arm.joint_efforts(
) + self.right_arm.joint_efforts()
else:
raise ValueError("Arg arm should be 'left' or 'right' or 'both'.")
if not self.sim:
joint_efforts = joint_efforts.values()
return
def apply_action(self, arm, control_type, action):
"""
Apply a joint action
Blocking on real robot
Args
action - list or tuple specifying end effector pose(6DOF * num_arms)
or joint angles (7DOF * num_arms)
"""
# verify action
verified, err = self._verify_action(arm, control_type, action)
if not verified:
raise err
# execute action
if control_type == 'position':
self._apply_position_control(arm, action)
elif control_type == 'ee':
self._apply_ee_control(arm, action)
elif control_type == 'velocity':
self._apply_velocity_control(arm, action)
elif control_type == 'torque':
self._apply_torque_control(arm, action)
else:
raise ValueError(
"Control type must be ['ee', 'position', 'velocity', 'torque']"
)
def _verify_action(self, arm, control_type, action):
"""
Verify type and dimension of action
Args
arm (str): "left" or "right" or "both"
action (list): list of floats len will vary depending on action type
Returns
bool: True if action is right dimension, false otherwise
"""
if arm not in ["left", "right", "both"]:
return False, ValueError("Arg arm must be string")
if control_type not in ['ee', 'position', 'velocity', 'torque']:
return False, ValueError(
"Arg control_type must be one of ['ee', 'position', 'velocity', 'torque']"
)
if not isinstance(action, (list, tuple, np.ndarray)):
return False, TypeError("Action must be a list or tuple.")
if len(action) != self.dof[arm][control_type]:
return False, ValueError("Action must have len {}".format(
self.dof * num_arms))
return True, ""
def _apply_torque_control(self, arm, action):
"""
As of right now, setting joint torques does not
does not command the robot as expected
"""
raise NotImplementedError(
'Cannot apply torque control. Try using joint position or velocity control'
)
def _apply_velocity_control(self, arm, action):
"""
Apply velocity control to a given arm
Args
arm (str): 'right' or 'left'
action (list, tuple, or numpy array) of len 7
"""
if self.sim:
pass
else:
if arm == 'left':
action_dict = self
self.right_arm.set_joint_velocities
if arm == 'right':
pass
if arm == 'both':
pass
return
def _apply_position_control(self, arm, action, blocking=True):
"""
Apply a joint action
Args:
arm (str): 'right', 'left', or 'both'
action - list or array of joint angles
blocking (bool): If true, wait for arm(s) to reach final position(s)
"""
action = list(action)
if blocking:
self.move_to_joint_positions(arm, action)
else:
self.set_joint_positions(arm, action)
return
def _apply_ee_control(self, arm, action, blocking=True):
"""
Apply action to move Baxter end effector(s)
Args
arm (str): 'left' or 'right' or 'both'
action (list, tuple, or numpy array)
blocking: Bool
"""
action = list(action)
if self.sim:
if arm == 'left' or arm == 'right':
self.move_to_ee_pose(arm, action)
elif arm == 'both':
if self.sim:
joint_indices = self.left_arm.indices + self.right_arm.indices
self.left_arm.move_to_joint_positions(
actions, joint_indices)
else:
self.move_to_ee_pose(arm, action, blocking)
return
def fk(self, arm, joints):
"""
Calculate forward kinematics
Args
arm (str): 'right' or 'left'
joints (list): list of joint angles
Returns
pose(list): [x,y,z,r,p,w]
"""
if arm == 'right':
pose = list(self.right_arm.kin.forward_position_kinematics(joints))
elif arm == 'left':
pose = list(self.left_arm.kin.forward_position_kinematics(joints))
else:
raise ValueError("Arg arm must be 'right' or 'left'")
return pose[:3] + list(self.quat_to_euler(pose[3:]))
def _ik(self, arm, pos, orn=None, seed=None):
"""
Calculate inverse kinematics
Args
arm (str): 'right' or 'left'
pos (list): [X, Y, Z]
orn (list): [r, p, w]
seed (int): for setting random seed
Returns
joint angles (list): A list of joint angles
Note: This will probably fail if orn is not included
"""
if orn is not None:
orn = list(self.euler_to_quat(orn))
if arm == 'right':
joint_angles = self.right_arm.kin.inverse_kinematics(pos, orn)
elif arm == 'left':
joint_angles = self.left_arm.kin.inverse_kinematics(pos, orn, seed)
else:
raise ValueError("Arg arm must be 'right' or 'left'")
return list(joint_angles.tolist())
def ik(self, arm, ee_pose):
"""
Calculate inverse kinematics for a given end effector pose
Args
ee_pose (list or tuple of floats): 6 dimensional array specifying
the position and orientation of the end effector
arm (string): "right" or "left"
Return
joints (list): A list of joint angles
"""
if self.sim:
joints = self._sim_ik(arm, ee_pose)
else:
joints = self._real_ik(arm, ee_pose)
if not joints:
print("IK failed. Try running again or changing the pose.")
return joints
def _sim_ik(self, arm, ee_pose):
"""
(Sim) Calculate inverse kinematics for a given end effector pose
Args:
ee_pose (tuple or list): [pos, orn] of desired end effector pose
pos - x,y,z
orn - r,p,w
arm (string): "right" or "left"
Returns:
joint_angles (list): List of joint angles
"""
if arm == 'right':
ee_index = self.right_arm.ee_index
elif arm == 'left':
ee_index = self.left_arm.ee_index
elif arm == 'both':
pass
else:
raise ValueError("Arg arm must be 'left', 'right', or 'both'.")
pos = ee_pose[:3]
orn = ee_pose[3:]
if (self.use_nullspace == 1):
if (self.use_orientation == 1):
joints = p.calculateInverseKinematics(self.baxter_id, ee_index,
pos, orn, self.ll,
self.ul, self.jr,
self.rp)
else:
joints = p.calculateInverseKinematics(self.baxter_id,
ee_index,
pos,
lowerLimits=self.ll,
upperLimits=self.ul,
jointRanges=self.jr,
restPoses=self.rp)
else:
if (self.use_orientation == 1):
joints = p.calculateInverseKinematics(self.baxter_id,
ee_index,
pos,
orn,
jointDamping=self.jd)
else:
joints = p.calculateInverseKinematics(self.baxter_id, ee_index,
pos)
return joints
def _real_ik(self, arm, ee_pose):
"""
(Real) Calculate inverse kinematics for a given end effector pose
Args:
ee_pose (tuple or list): [pos, orn] of desired end effector pose
pos - x,y,z
orn - r,p,w
Returns:
joint_angles (dict): Dictionary containing {'joint_name': joint_angle}
"""
pos = ee_pose[:3]
orn = ee_pose[3:]
orn = self.euler_to_quat(orn)
ns = "ExternalTools/" + arm + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
ik_pose = PoseStamped()
ik_pose.pose.position.x = pos[0]
ik_pose.pose.position.y = pos[1]
ik_pose.pose.position.z = pos[2]
ik_pose.pose.orientation.x = orn[0]
ik_pose.pose.orientation.y = orn[1]
ik_pose.pose.orientation.z = orn[2]
ik_pose.pose.orientation.w = orn[3]
ik_pose.header = hdr
ikreq.pose_stamp.append(ik_pose)
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
limb_joints = dict(
zip(resp.joints[0].name, resp.joints[0].position))
return limb_joints
except (rospy.ServiceException, rospy.ROSException), e:
# rospy.logerr("Service call failed: %s" % (e,))
return
class ArmController(object):
def __init__(self,
starting_poss=None,
push_thresh=10,
mode='one_arm',
arm_mode='first'):
"""
Initialises parameters and moves the arm to a neutral
position.
"""
self.push_thresh = push_thresh
self._right_neutral_pos = starting_poss[0]
self._left_neutral_pos = starting_poss[1]
self._mode = mode
self._arm_mode = arm_mode
rospy.loginfo("Creating interface and calibrating gripper")
self._right_limb = Limb('right')
self._left_limb = Limb('left')
self._right_gripper = Gripper('right', CHECK_VERSION)
self._right_gripper.calibrate()
self._left_gripper = Gripper('left', CHECK_VERSION)
self._left_gripper.calibrate()
self._is_right_fist_closed = False
self._is_left_fist_closed = False
rospy.loginfo("Moving to neutral position")
self.move_to_neutral()
rospy.loginfo("Initialising PoseGenerator")
self._pg = PoseGenerator(self._mode, self._arm_mode)
self._sub_right_gesture = rospy.Subscriber(
"/low_myo/gesture", UInt8, self._right_gesture_callback)
self._sub_left_gesture = rospy.Subscriber("/top_myo/gesture", UInt8,
self._left_gesture_callback)
self._last_data = None
self._pg.calibrate()
def move_to_neutral(self):
if self._mode == "one_arm":
self._right_limb.move_to_joint_positions(self._right_neutral_pos)
elif self._mode == "two_arms":
self._right_limb.move_to_joint_positions(self._right_neutral_pos)
self._left_limb.move_to_joint_positions(self._left_neutral_pos)
else:
raise ValueError("Mode %s is invalid!" % self._mode)
def is_right_pushing(self):
"""
Checks if any of the joints is under external stress. Returns
true if the maximum recorded stress above specified threshold.
"""
e = self._right_limb.joint_efforts()
max_effort = max([abs(e[i]) for i in e.keys()])
return max_effort > self.push_thresh
def is_left_pushing(self):
"""
Checks if any of the joints is under external stress. Returns
true if the maximum recorded stress above specified threshold.
"""
e = self._left_limb.joint_efforts()
max_effort = max([abs(e[i]) for i in e.keys()])
return max_effort > self.push_thresh
def _command_right_gripper(self):
"""
Reads state from Myo and opens/closes gripper as needed.
"""
if not self._right_gripper.ready():
return
if self._right_gripper.moving():
return
if self._is_right_fist_closed:
self._right_gripper.close()
else:
self._right_gripper.open()
def _command_left_gripper(self):
"""
Reads state from Myo and opens/closes gripper as needed.
"""
if not self._left_gripper.ready():
return
if self._left_gripper.moving():
return
if self._is_left_fist_closed:
self._left_gripper.close()
else:
self._left_gripper.open()
def step(self):
"""
Executes a step of the main routine.
Fist checks the status of the gripper and
"""
os.system('clear')
if self._mode == "one_arm":
return self.one_arm_step()
elif self._mode == "two_arms":
return self.two_arms_step()
else:
raise ValueError("Mode %s is invalid!" % self.mode)
def one_arm_step(self):
self._command_right_gripper()
pos = self._pg.generate_pose()
if pos is not None:
if not self.is_right_pushing():
self._right_limb.set_joint_positions(pos)
else:
rospy.logwarn("Arm is being pushed!")
else:
rospy.logwarn("Generated position is invalid")
def two_arms_step(self):
self._command_right_gripper()
self._command_left_gripper()
poss = self._pg.generate_pose()
if poss is not None:
if not self.is_right_pushing():
self._right_limb.set_joint_positions(poss[0])
if not self.is_left_pushing():
self._left_limb.set_joint_positions(poss[1])
else:
rospy.logwarn("Arm is being pushed!")
else:
rospy.logwarn("Generated position is invalid")
def _right_gesture_callback(self, data):
self._is_right_fist_closed = (data.data == 1)
def _left_gesture_callback(self, data):
self._is_left_fist_closed = (data.data != 0)
def listener():
global watch_timesteps
global firt_pack_sync
seq_len = 25
rospy.init_node('auto_grasping', anonymous=True, disable_signals=True)
model = load_model(pkg_dir + '/model/my_model25-94.h5')
zscore_data = np.loadtxt(pkg_dir + '/model/mean_std_zscore',
delimiter=',',
ndmin=2)
left_arm = Limb('left')
left_gripper = Gripper('left')
left_gripper.calibrate()
rate = rospy.Rate(50) # rate
rospy.Subscriber('/imu_giver', ImuPackage, watchCallback)
with firt_pack_sync:
firt_pack_sync.wait()
opened_timeout = 0
pre_res = 0
watch_buffer = []
bax_timesteps = []
# bax_timesteps and watch_buffer are two buffers to manage sequences
while not rospy.is_shutdown():
rate.sleep()
l_ang = list(left_arm.joint_angles().values())
l_vel = list(left_arm.joint_velocities().values())
l_eff = list(left_arm.joint_efforts().values())
bax_step = l_ang + l_vel + l_eff
bax_timesteps.append(bax_step)
if (watch_timesteps[1]):
watch_buffer.extend(watch_timesteps[0])
watch_timesteps[1] = 0
if (len(bax_timesteps) >= seq_len and len(watch_buffer) >= seq_len):
watch_buffer = watch_buffer[len(watch_buffer) - (seq_len):]
bax_timesteps = bax_timesteps[len(bax_timesteps) - (seq_len):]
sequence = []
for i in range(0, math.floor(seq_len * 0.3)):
step = watch_buffer.pop(0) + bax_timesteps.pop(0)
sequence.append(step)
for i in range(0, math.ceil(seq_len * 0.7)):
step = watch_buffer[i] + bax_timesteps[i]
sequence.append(step)
sequence = np.array(sequence)
sequence = sequence - zscore_data[0, :]
sequence = sequence / zscore_data[1, :]
seq = np.ndarray((1, seq_len, sequence.shape[1]))
seq[0] = sequence
res = model.predict(seq)
res = res[0][0]
rospy.loginfo(left_gripper.position())
if (left_gripper.position() > 94.0):
opened_timeout = opened_timeout + 1
if (res > 0.7 and pre_res > 0.7 and opened_timeout > 25):
left_gripper.command_position(0.0)
opened_timeout = 0
pre_res = res
def listener():
rospy.init_node('record_data', anonymous=True, disable_signals=True)
global BAX_COLUMNS
global WATCH_COLUMNS
global NANOS_TO_MILLIS
global bax_file_name
global bax_row
global watch_rows
global command
global sequence_counter
resetNode()
rospy.loginfo("Commands :\ns to stop\nr to remove the last file\nn to start new sequence\nc TWICE to shutdown the node\n")
rate = rospy.Rate(120) # rate
watch_sub = rospy.Subscriber('/imu_giver', ImuPackage, watchCallback)
rospy.loginfo("START RECORDING SEQUENCE " + str(sequence_counter))
getkey_thread = Thread(target = getKey)
getkey_thread.start()
left_arm = Limb('left')
left_gripper = Gripper('left')
while not rospy.is_shutdown():
rate.sleep()
t = round(rospy.get_rostime().to_nsec()*NANOS_TO_MILLIS)
l_ang = list(left_arm.joint_angles().values())
l_vel = list(left_arm.joint_velocities().values())
l_eff = list(left_arm.joint_efforts().values())
l_grip_pos = str(left_gripper.position())
bax_row = l_ang + l_vel + l_eff
bax_row.append(l_grip_pos)
bax_row.append(str(t))
with open(bax_file_name + str(sequence_counter), 'a') as writeFile:
writer = csv.writer(writeFile)
writer.writerow(bax_row)
writeFile.close()
if (watch_rows[1]==1):
with open(watch_file_name + str(sequence_counter), 'a') as writeFile:
writer = csv.writer(writeFile)
writer.writerows(watch_rows[0])
writeFile.close()
watch_rows[1]=0
# s to stop
# r to remove the last file
# n to start new sequence
# c TWICE to shutdown the node
shutdown = False
if (command == 's') :
watch_sub.unregister()
rospy.loginfo("FINISH RECORDING SEQUENCE " + str(sequence_counter))
rospy.loginfo("NODE STOPPED!")
while True :
rospy.Rate(2).sleep()
if (command == 'r') :
os.remove(bax_file_name + str(sequence_counter))
os.remove(watch_file_name + str(sequence_counter))
sequence_counter = sequence_counter - 1
rospy.loginfo("FILE REMOVED!")
command = ''
if (command == 'n') :
rospy.loginfo("RESET NODE!")
sequence_counter = sequence_counter + 1
resetNode()
watch_sub = rospy.Subscriber('/imu_giver', ImuPackage, watchCallback)
rospy.loginfo("START RECORDING SEQUENCE " + str(sequence_counter))
break
if (command == 'c') :
rospy.loginfo("Enter 'c' to shutdown... ")
shutdown = True
break
if (shutdown) :
rospy.signal_shutdown("reason...")
getkey_thread.join()
def main():
rospy.init_node('baxter_kinematics')
kin = baxter_kinematics('left')
pos = [0.582583, -0.180819, 0.216003]
rot = [0.03085, 0.9945, 0.0561, 0.0829]
fl = fcntl.fcntl(sys.stdin.fileno(), fcntl.F_GETFL)
fcntl.fcntl(sys.stdin.fileno(), fcntl.F_SETFL, fl | os.O_NONBLOCK)
# Read initial positions:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm = Limb('right')
left_arm = Limb('left')
joints = left_arm.joint_names()
positions = left_arm.joint_angles()
velocities = left_arm.joint_velocities()
force = convert_dict_to_list('left', left_arm.joint_efforts())
# Initial states
q_previous = positions # Starting joint angles
q_dot_previous = velocities # Starting joint velocities
x_previous = kin.forward_position_kinematics(
) # Starting end effector position
x_dot_previous = np.zeros((6, 1))
# Set model parameters:
m = 1
K = 0.2
C = 15
B = 12
while True:
'''
Following code breaks loop upon user input (enter key)
'''
try:
stdin = sys.stdin.read()
if "\n" in stdin or "\r" in stdin:
return_to_init(joints, 'left')
break
except IOError:
pass
# Gather Jacobian information:
J = kin.jacobian()
J_T = kin.jacobian_transpose()
J_PI = kin.jacobian_pseudo_inverse()
J_T_PI = np.linalg.pinv(J_T)
# Extract sensor data:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm = Limb('right')
left_arm = Limb('left')
# Information is published at 100Hz by default (every 10ms=.01sec)
time_step = 0.01
joints = left_arm.joint_names()
positions = convert_dict_to_list('left', left_arm.joint_angles())
velocities = convert_dict_to_list('left', left_arm.joint_velocities())
force = convert_dict_to_list('left', left_arm.joint_efforts())
force_mag = np.linalg.norm(force)
print(force_mag)
if (force_mag < 28): # Add deadzone
continue
else:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm = Limb('right')
left_arm = Limb('left')
joints = left_arm.joint_names()
positions = convert_dict_to_list('left', left_arm.joint_angles())
velocities = convert_dict_to_list('left',
left_arm.joint_velocities())
force = convert_dict_to_list('left', left_arm.joint_efforts())
positions = np.reshape(positions, [7, 1]) # Converts to matrix
velocities = np.reshape(velocities, [7, 1]) # Converts to matrix
force = np.reshape(force, [7, 1]) # Converts to matrix
x = kin.forward_position_kinematics()
x = x[:-1]
x_ref = np.reshape(x, [6, 1]) # Reference position
x_ref_dot = J * velocities # Reference velocities
t_stop = 10
t_end = time.time() + t_stop # Loop timer (in seconds)
# Initial plot parameters
time_plot_cum = 0
time_vec = [time_plot_cum]
pos_vec = [x_ref[1][0]]
# For integral control
x_ctrl_cum = 0
time_cum = 0.00001 # Prevent divide by zero
x_ctrl_prev = 0 # Initial for derivative ctrl
while time.time() < t_end:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm = Limb('right')
left_arm = Limb('left')
J = kin.jacobian()
J_T = kin.jacobian_transpose()
J_PI = kin.jacobian_pseudo_inverse()
J_T_PI = np.linalg.pinv(J_T)
joints = left_arm.joint_names()
positions = convert_dict_to_list('left',
left_arm.joint_angles())
velocities = convert_dict_to_list('left',
left_arm.joint_velocities())
force = convert_dict_to_list('left', left_arm.joint_efforts())
positions = np.reshape(positions, [7, 1]) # Converts to matrix
velocities = np.reshape(velocities,
[7, 1]) # Converts to matrix
force = np.reshape(force, [7, 1]) # Converts to matrix
x = kin.forward_position_kinematics()
x = x[:-1]
x_current = np.reshape(x, [6, 1])
x_dot_current = J * velocities
# Only interested in y-axis:
x_dot_current[0] = 0
#x_dot_current[1]=0
x_dot_current[2] = 0
x_dot_current[3] = 0
x_dot_current[4] = 0
x_dot_current[5] = 0
x_ddot = derivative(x_dot_previous, x_dot_current, time_step)
# Model goes here
f = J_T_PI * force # spacial force
# Only interested in y-axis:
f[0] = [0]
#f[1]=[0]
f[2] = [0]
f[3] = [0]
f[4] = [0]
f[5] = [0]
x_des = ((B * f - m * x_ddot - C *
(x_dot_current - x_ref_dot)) /
K) + x_ref # Spring with damper
# Control robot
Kp = 0.0004
Ki = 0.00000022
Kd = 0.0000005
x_ctrl = x_current - x_des
# Only interested in y-axis:
x_ctrl[0] = 0
#x_ctrl[1]=0
x_ctrl[2] = 0
x_ctrl[3] = 0
x_ctrl[4] = 0
x_ctrl[5] = 0
q_dot_ctrl = J_T * np.linalg.inv(J * J_T + np.identity(6)) * (
-Kp * x_ctrl - Ki * (x_ctrl_cum) - Kd *
(x_ctrl_prev - x_ctrl) / time_step)
q_dot_ctrl_list = convert_mat_to_list(q_dot_ctrl)
q_dot_ctrl_dict = convert_list_to_dict(joints, q_dot_ctrl_list)
left_arm.set_joint_velocities(
q_dot_ctrl_dict) # Velocity control function
# Update plot info
time_cum += .05
time_vec.append(time_cum)
pos_vec.append(x_current[1][0])
# Update integral control parameters
x_ctrl_cum += x_ctrl * time_step
# Update derivative control parameters
x_ctrl_prev = x_ctrl
# Update values before next loop
x_previous = x_current # Updates previous position for next loop iteration
x_dot_previous = x_dot_current # Updates previous velocity for next loop iteration
print(time_vec)
print(pos_vec)
plt.plot(time_vec, pos_vec)
plt.title('Position over time')
plt.xlabel('Time (sec)')
plt.ylabel('Position')
plt.show()
stop_joints(q_dot_ctrl_dict)
left_arm.set_joint_velocities(q_dot_ctrl_dict)
time.sleep(1)
break
def main():
rospy.init_node('baxter_kinematics')
kin = baxter_kinematics('left')
pos = [0.582583, -0.180819, 0.216003]
rot = [0.03085, 0.9945, 0.0561, 0.0829]
fl = fcntl.fcntl(sys.stdin.fileno(), fcntl.F_GETFL)
fcntl.fcntl(sys.stdin.fileno(), fcntl.F_SETFL, fl | os.O_NONBLOCK)
# Read initial positions:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm=Limb('right')
left_arm=Limb('left')
joints=left_arm.joint_names()
positions=convert_dict_to_list('left',left_arm.joint_angles())
velocities=convert_dict_to_list('left',left_arm.joint_velocities())
force=convert_dict_to_list('left',left_arm.joint_efforts())
positions=np.reshape(positions,[7,1]) # Converts to matrix
velocities=np.reshape(velocities,[7,1]) # Converts to matrix
force=np.reshape(force,[7,1]) # Converts to matrix
# Initial states
q_previous=positions # Starting joint angles
q_dot_previous=velocities # Starting joint velocities
x_previous=kin.forward_position_kinematics() # Starting end effector position
x_dot_previous=np.zeros((6,1))
# Set model parameters:
C=50
B=1
f_des=np.reshape(np.array([0,15,0,0,0,0]),[6,1])
J=kin.jacobian()
J_T=kin.jacobian_transpose()
J_PI=kin.jacobian_pseudo_inverse()
J_T_PI=np.linalg.pinv(J_T)
x=kin.forward_position_kinematics()
x=x[:-1]
x_ref=np.reshape(x,[6,1]) # Reference position
x_ref_dot=J*velocities # Reference velocities
t_stop=15
t_end=time.time()+t_stop # Loop timer (in seconds)
# Initial plot parameters
time_cum=0
time_vec=[time_cum]
f=J_T_PI*force
force_vec=[convert_mat_to_list(f[1])[0]]
while time.time()<t_end:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm=Limb('right')
left_arm=Limb('left')
J=kin.jacobian()
J_T=kin.jacobian_transpose()
J_PI=kin.jacobian_pseudo_inverse()
J_T_PI=np.linalg.pinv(J_T)
joints=left_arm.joint_names()
positions=convert_dict_to_list('left',left_arm.joint_angles())
velocities=convert_dict_to_list('left',left_arm.joint_velocities())
force=convert_dict_to_list('left',left_arm.joint_efforts())
positions=np.reshape(positions,[7,1]) # Converts to matrix
velocities=np.reshape(velocities,[7,1]) # Converts to matrix
force=np.reshape(force,[7,1]) # Converts to matrix
x=kin.forward_position_kinematics()
x=x[:-1]
x_current=np.reshape(x,[6,1])
x_dot_current=J*velocities
# Only interested in y-axis:
x_dot_current[0]=0
#x_dot_current[1]=0
x_dot_current[2]=0
x_dot_current[3]=0
x_dot_current[4]=0
x_dot_current[5]=0
# Model goes here
f=J_T_PI*force # spacial force
# Only interested in y-axis:
f[0]=[0]
#f[1]=[0]
f[2]=[0]
f[3]=[0]
f[4]=[0]
f[5]=[0]
x_dot_des=-B*(f_des+f)/C # Impedance control
# Control robot
q_dot_ctrl=J_PI*x_dot_des # Use this for damper system
q_dot_ctrl=np.multiply(q_dot_ctrl,np.reshape(np.array([1,0,0,0,0,0,0]),[7,1])) # y-axis translation corresponds to first shoulder joint rotation
q_dot_ctrl_list=convert_mat_to_list(q_dot_ctrl)
q_dot_ctrl_dict=convert_list_to_dict(joints,q_dot_ctrl_list)
left_arm.set_joint_velocities(q_dot_ctrl_dict) # Velocity control function
# Update values before next loop
x_previous=x_current # Updates previous position for next loop iteration
x_dot_previous=x_dot_current # Updates previous velocity for next loop iteration
# Update plot info
time_cum+=.05
time_vec.append(time_cum)
force_vec.append(convert_mat_to_list(f[1])[0])
print(time_vec)
print(force_vec)
plt.plot(time_vec,force_vec)
plt.title('Force applied over time')
plt.xlabel('Time (sec)')
plt.ylabel('Force (N)')
plt.show()
time.sleep(1)
