def main():
sim = True
# instantiate an ada
ada = adapy.Ada(True)
armHome = [-1.5, 3.22, 1.23, -2.19, 1.8, 1.2]
goalConfig = [-1.72, 4.44, 2.02, -2.04, 2.66, 1.39]
delta = 0.25
eps = 0.2
if sim:
ada.set_positions(goalConfig)
# launch viewer
viewer = ada.start_viewer("dart_markers/simple_trajectories", "map")
# add objects to world
canURDFUri = "package://pr_assets/data/objects/can.urdf"
sodaCanPose = [0.25, -0.35, 0.0, 0, 0, 0, 1]
tableURDFUri = "package://pr_assets/data/furniture/uw_demo_table.urdf"
tablePose = [0.3, 0.0, -0.7, 0.707107, 0, 0, 0.707107]
world = ada.get_world()
can = world.add_body_from_urdf(canURDFUri, sodaCanPose)
table = world.add_body_from_urdf(tableURDFUri, tablePose)
# add collision constraints
collision_free_constraint = ada.set_up_collision_detection(
ada.get_arm_state_space(), ada.get_arm_skeleton(), [can, table])
full_collision_constraint = ada.get_full_collision_constraint(
ada.get_arm_state_space(), ada.get_arm_skeleton(),
collision_free_constraint)
# easy goal
adaRRT = AdaRRT(start_state=np.array(armHome),
goal_state=np.array(goalConfig),
ada=ada,
ada_collision_constraint=full_collision_constraint,
step_size=delta,
goal_precision=eps)
rospy.sleep(1.0)
path = adaRRT.build()
if path is not None:
print("Path waypoints:")
print(np.asarray(path))
waypoints = []
for i, waypoint in enumerate(path):
waypoints.append((0.0 + i, waypoint))
t0 = time.clock()
traj = ada.compute_smooth_joint_space_path(ada.get_arm_state_space(),
waypoints)
t = time.clock() - t0
print(str(t) + "seconds elapsed")
raw_input('Press ENTER to execute trajectory and exit')
ada.execute_trajectory(traj)
rospy.sleep(1.0)
#!/usr/bin/env python
import adapy
import rospy
import sys
import pdb
from moveit_ros_planning_interface._moveit_roscpp_initializer import roscpp_init
rospy.init_node("adapy_simple_traj")
roscpp_init('adapy_simple_traj', [])
rate = rospy.Rate(10)
IS_SIM = True
if not rospy.is_shutdown():
ada = adapy.Ada(IS_SIM)
if not IS_SIM:
if not ada.start_trajectory_controllers():
print("Could not start trajectory controller.")
sys.exit(1)
rospy.sleep(1) # wait for ada to initialize
viewer = ada.start_viewer("dart_markers/simple_trajectories", "map")
canURDFUri = "package://pr_assets/data/objects/can.urdf"
sodaCanPose = [1.0, 0.0, 0.73, 0, 0, 0, 1]
tableURDFUri = "package://pr_assets/data/furniture/uw_demo_table.urdf"
tablePose = [1., 0.0, 0.0, 0.707107, 0, 0, 0.707107]
world = ada.get_world()
can = world.add_body_from_urdf(canURDFUri, sodaCanPose)
table = world.add_body_from_urdf(tableURDFUri, tablePose)
#!/usr/bin/env python
import adapy
import rospy
import pdb
pdb.set_trace()
rospy.init_node("adapy_simple_traj")
rate = rospy.Rate(10)
if not rospy.is_shutdown():
ada = adapy.Ada(True)
viewer = ada.start_viewer("dart_markers/simple_trajectories", "map")
canURDFUri = "package://pr_assets/data/objects/can.urdf"
sodaCanPose = [1.0, 0.0, 0.73, 0, 0, 0, 1]
tableURDFUri = "package://pr_assets/data/furniture/uw_demo_table.urdf"
tablePose = [1., 0.0, 0.0, 0.707107, 0, 0, 0.707107]
world = ada.get_world()
can = world.add_body_from_urdf(canURDFUri, sodaCanPose)
table = world.add_body_from_urdf(tableURDFUri, tablePose)
collision = ada.get_self_collision_constraint()
arm_skeleton = ada.get_arm_skeleton()
positions = arm_skeleton.get_positions()
arm_state_space = ada.get_arm_state_space()
positions2 = positions.copy()
positions3 = positions.copy()
positions4 = positions.copy()
def main(if_sim):
# initialize roscpp, if it's for real robot
# http://wiki.ros.org/ROS/Tutorials/Using%20a%20C%2B%2B%20class%20in%20Python
if not if_sim:
from moveit_ros_planning_interface._moveit_roscpp_initializer import roscpp_init
roscpp_init('soda_grasp_ik', [])
# instantiate an ada
ada = adapy.Ada(if_sim)
# launch viewer
viewer = ada.start_viewer("dart_markers/sode_grasp", "map")
world = ada.get_world()
hand = ada.get_hand()
hand_node = hand.get_endeffector_body_node()
arm_skeleton = ada.get_arm_skeleton()
arm_state_space = ada.get_arm_state_space()
# joint positions of the starting pose
arm_home = [-1.5, 3.22, 1.23, -2.19, 1.8, 1.2]
if if_sim:
ada.set_positions(arm_home)
else:
raw_input("Please move arm to home position with the joystick. " +
"Press ENTER to continue...")
viewer.add_frame(hand_node)
# add objects to world
soda_pose = np.eye(4)
soda_pose[0, 3] = 0.25
soda_pose[1, 3] = -0.35
sodaURDFUri = "package://pr_assets/data/objects/can.urdf"
soda = world.add_body_from_urdf_matrix(sodaURDFUri, soda_pose)
tableURDFUri = "package://pr_assets/data/furniture/uw_demo_table.urdf"
# x, y, z, rw, rx, ry, rz
table_pose = [0.3, 0.0, -0.75, 0.707107, 0., 0., 0.707107]
table = world.add_body_from_urdf(tableURDFUri, table_pose)
# add collision constraints
collision_free_constraint = ada.set_up_collision_detection(
ada.get_arm_state_space(), ada.get_arm_skeleton(), [soda, table])
full_collision_constraint = ada.get_full_collision_constraint(
ada.get_arm_state_space(), ada.get_arm_skeleton(),
collision_free_constraint)
rospy.sleep(1.)
raw_input("Press ENTER to generate the TSR...")
# create TSR
sodaTSR = createSodaTSR(soda_pose, hand)
marker = viewer.add_tsr_marker(sodaTSR)
raw_input("Press ENTER to start planning goals...")
# set up IK generator
ik_sampleable = adapy.create_ik(arm_skeleton, arm_state_space, sodaTSR,
hand_node)
ik_generator = ik_sampleable.create_sample_generator()
configurations = []
samples = 0
maxSamples = 10
while samples < maxSamples and ik_generator.can_sample():
goal_state = ik_generator.sample(arm_state_space)
samples += 1
if len(goal_state) == 0:
continue
configurations.append(goal_state)
if len(configurations) == 0:
print("No valid configurations found!")
if if_sim:
ada.set_positions(arm_home)
raw_input("Press ENTER to start RRT planning...")
trajectory = None
for configuration in configurations:
# Your AdaRRT planner
### FILL in your code here
if trajectory:
break
if not trajectory:
print("Failed to find a solution!")
sys.exit(1)
else:
print("Found a trajectory!")
# smooth the RRTs trajectory
shortcut(trajectory, ada, full_collision_constraint)
waypoints = []
for i, waypoint in enumerate(trajectory):
waypoints.append((0.0 + i, waypoint))
# compute trajectory in joint space
t0 = time.clock()
traj = ada.compute_joint_space_path(ada.get_arm_state_space(), waypoints)
retimed_traj = ada.compute_retime_path(ada.get_arm_skeleton(), traj)
t = time.clock() - t0
print(str(t) + "seconds elapsed")
raw_input('Press ENTER to execute the trajectory...')
# execute the trajectory
if not if_sim:
ada.start_trajectory_executor()
ada.execute_trajectory(retimed_traj)
raw_input('Press ENTER after robot has approached the can...')
if not if_sim:
ada.set_positions(waypoints[-1][1])
# execute the grasp
print("Closing hand")
### FILL in your code here
raw_input('Press ENTER after robot has succeeded closing the hand...')
if if_sim:
hand.grab(soda)
# compute the Jacobian pseudo-inverse for moving the hand upwards
if if_sim:
N = 50
Dz = 0.01
# waypoints = []
for i in range(N):
q = arm_skeleton.get_positions()
### FILL in your code here
ada.set_positions(q)
viewer.update()
time.sleep(0.05)
else:
# in real world
q = arm_skeleton.get_positions()
jac = arm_skeleton.get_jacobian(hand.get_endeffector_body_node())
delta_x = np.array([0, 0, 0, 0.5, 0, 0])
delta_q = np.matmul(np.linalg.pinv(jac), delta_x)
q = q + delta_q
traj = ada.plan_to_configuration(ada.get_arm_state_space(),
ada.get_arm_skeleton(), q)
retimed_traj = ada.compute_retime_path(ada.get_arm_skeleton(), traj)
raw_input('Press ENTER to execute the lift')
ada.execute_trajectory(retimed_traj)
raw_input('Press ENTER after robot has succeeded lifting up the can...')
# clean the scene
world.remove_skeleton(soda)
world.remove_skeleton(table)
