def main():
"""
This function shows an example of block stacking.
"""
np.set_printoptions(precision=4, suppress=True)
robot = Robot('franka')
success = robot.arm.go_home()
if not success:
log_warn('Robot go_home failed!!!')
ori = euler2quat([0, 0, np.pi / 2])
robot.pb_client.load_urdf('table/table.urdf',
[.6, 0, 0.4],
ori,
scaling=0.9)
box_size = 0.03
box_id1 = robot.pb_client.load_geom('box', size=box_size,
mass=0.1,
base_pos=[.5, 0.12, 1.0],
rgba=[1, 0, 0, 1])
box_id2 = robot.pb_client.load_geom('box',
size=box_size,
mass=0.1,
base_pos=[0.3, 0.12, 1.0],
rgba=[0, 0, 1, 1])
robot.arm.eetool.open()
obj_pos = robot.pb_client.get_body_state(box_id1)[0]
move_dir = obj_pos - robot.arm.get_ee_pose()[0]
move_dir[2] = 0
eef_step = 0.025
# an example of using IK with nullspace enabled
ik_kwargs = dict(ns=True)
robot.arm.move_ee_xyz(move_dir, eef_step=eef_step, **dict(ik_kwargs=ik_kwargs))
move_dir = np.zeros(3)
move_dir[2] = obj_pos[2] - robot.arm.get_ee_pose()[0][2]
robot.arm.move_ee_xyz(move_dir, eef_step=eef_step)
robot.arm.eetool.close(wait=False)
robot.arm.move_ee_xyz([0, 0, 0.3], eef_step=eef_step)
obj_pos = robot.pb_client.get_body_state(box_id2)[0]
move_dir = obj_pos - robot.arm.get_ee_pose()[0]
move_dir[2] = 0
robot.arm.move_ee_xyz(move_dir, eef_step=eef_step)
move_dir = obj_pos - robot.arm.get_ee_pose()[0]
move_dir[2] += box_size * 2
robot.arm.move_ee_xyz(move_dir, eef_step=eef_step)
robot.arm.eetool.open()
move_dir[2] = 0.2
robot.arm.move_ee_xyz(move_dir, eef_step=eef_step)
time.sleep(10)
def is_jpos_in_good_range(self):
"""
Check if the joint angles lie in (-pi, pi].
Returns:
bool: whether the joint angles are in (-pi, pi].
"""
jposs = self.get_jpos()
for i, jpos in enumerate(jposs):
if jpos <= -np.pi or jpos > np.pi:
ar.log_warn('Current joint angles are: %s\n'
'Some joint angles are outside of the valid'
' range (-pi, pi]\n Please use the Teaching'
' Pendant to move the correponding joints so'
' that all the joint angles are within (-pi,'
' pi]!' % str(jposs))
return False
return True
def set_jvel(self, velocity, joint_name=None, wait=False, *args, **kwargs):
"""
Set joint velocity command to the robot (units in rad/s).
Args:
velocity (float or list or flattened np.ndarray): list of target
joint velocity value(s)
(shape: :math:`[6,]` if list, otherwise a single value)
joint_name (str, optional): If not provided, velocity should be
list and all joints will be turned on at specified velocity.
Defaults to None.
wait (bool, optional): If True, block until robot reaches
desired joint velocity value(s). Defaults to False.
Returns:
bool: True if command was completed successfully, returns
False if wait flag is set to False.
"""
velocity = copy.deepcopy(velocity)
success = False
# TODO improve velocity control performance
ar.log_warn('Velocity control is not well tested!!!')
if self._gazebo_sim:
raise NotImplementedError('cannot set_jvel in Gazebo')
if joint_name is None:
if len(velocity) != self.arm_dof:
raise ValueError('Velocity should contain %d elements'
'if the joint name is not '
'provided' % self.arm_dof)
tgt_vel = velocity
else:
if not isinstance(velocity, float):
raise TypeError('velocity should be individual float value'
'if joint_name is provided')
if joint_name not in self.arm_jnt_names_set:
raise TypeError('Joint name [%s] is not in the arm'
' joint list!' % joint_name)
else:
tgt_vel = [0.0] * len(self.arm_jnt_names)
arm_jnt_idx = self.arm_jnt_names.index(joint_name)
tgt_vel[arm_jnt_idx] = velocity
if self._use_urscript:
prog = 'speedj([%f, %f, %f, %f, %f, %f],' \
' a=%f)' % (tgt_vel[0],
tgt_vel[1],
tgt_vel[2],
tgt_vel[3],
tgt_vel[4],
tgt_vel[5],
self._motion_acc)
self._send_urscript(prog)
else:
# self._pub_joint_vel(tgt_vel)
raise NotImplementedError('set_jvel only works '
'with use_urscript=True')
if wait:
success = wait_to_reach_jnt_goal(
velocity,
get_func=self.get_jvel,
joint_name=joint_name,
timeout=self.cfgs.ARM.TIMEOUT_LIMIT,
max_error=self.cfgs.ARM.MAX_JOINT_VEL_ERROR)
return success
def main():
"""
This function shows an example of block stacking.
"""
np.set_printoptions(precision=4, suppress=True)
robot = Robot('ur5e_2f140', pb=True, pb_cfg={'gui': False})
success = robot.arm.go_home()
if not success:
ar.log_warn('Robot go_home failed!!!')
#setup cam
robot_base = robot.arm.robot_base_pos
robot.cam.setup_camera(focus_pt=robot_base,
dist=3,
yaw=55,
pitch=-30,
roll=0)
out = cv2.VideoWriter('pickandplace.mp4', cv2.VideoWriter_fourcc(*'mp4v'),
30, (640, 480))
img = robot.cam.get_images(get_rgb=True, get_depth=False)[0]
out.write(np.array(img))
ori = euler2quat([0, 0, np.pi / 2])
robot.pb_client.load_urdf('table/table.urdf', [.5, 0, 0.4],
ori,
scaling=0.9)
box_size = 0.05
box_id1 = robot.pb_client.load_geom('box',
size=box_size,
mass=1,
base_pos=[.5, 0.12, 1.0],
rgba=[1, 0, 0, 1])
robot.arm.eetool.open()
record(robot, out)
obj_pos = robot.pb_client.get_body_state(box_id1)[0]
move_dir = obj_pos - robot.arm.get_ee_pose()[0]
move_dir[2] = 0
eef_step = 0.025
for i in range(3):
robot.arm.move_ee_xyz(move_dir / 3, eef_step=eef_step)
record(robot, out)
move_dir = np.zeros(3)
move_dir[2] = obj_pos[2] - robot.arm.get_ee_pose()[0][2]
for i in range(4):
robot.arm.move_ee_xyz(move_dir / 4, eef_step=eef_step)
record(robot, out)
robot.arm.eetool.close(wait=False)
record(robot, out)
for i in range(10):
robot.arm.move_ee_xyz([0, 0, 0.03], eef_step=eef_step)
record(robot, out)
for i in range(10):
robot.arm.move_ee_xyz([0.025, 0, 0.0], eef_step=eef_step)
record(robot, out)
for i in range(10):
robot.arm.move_ee_xyz([0, 0, -0.03], eef_step=eef_step)
record(robot, out)
robot.arm.eetool.open()
record(robot, out)
time.sleep(1)
record(robot, out)
for i in range(10):
robot.arm.move_ee_xyz([0, 0, 0.03], eef_step=eef_step)
record(robot, out)
time.sleep(3)
record(robot, out)
out.release()