def __init__(self, local=True):
"""
Instantiates an interface for darias.
"""
# if not local
self._enabled = True
self._root = "/franka_ros_interface"
# if local
# self._root = "/panda_simulator"
# rospy.init_node("franka_robot_gym")
rospy.init_node("robopy_interface")
self.arms = Arms(self)
self._arm_interface = ArmInterface(True)
self._gripper_interface = GripperInterface()
self._robot_status = RobotEnable()
self._ctrl_manager = FrankaControllerManagerInterface(
ns=self._root, sim=False) # TODO: (sim=false for real robot))
print(self._ctrl_manager)
self._movegroup_interface = None # TODO: consider removing it
self._joint_command_publisher = rospy.Publisher(
self._root + '/motion_controller/arm/joint_commands',
# 'joint_commands',
JointCommand,
tcp_nodelay=True,
queue_size=1)
while not (self.arms.ready):
pass
self.groups = {}
self._building_groups()
# TODO: temporary
self._joint_names = self.groups["arm_gripper"].refs
def __init__(self, real_robot=False):
# Event is clear while initialization or set_state is going on
self.reset = Event()
self.reset.clear()
self.get_state_event = Event()
self.get_state_event.set()
self.real_robot = real_robot
# TODO publisher, subscriber, target and state
self._add_publishers()
self.panda_arm = ArmInterface()
self.target = [0.0] * 6 # TODO define number of target floats
# TODO define number of panda states (At least the number of joints)
self.panda_joint_names = [
'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4',
'panda_joint5', 'panda_joint6', 'panda_joint7'
]
self.panda_finger_names = [
'panda_finger_joint1', 'panda_finger_joint2'
]
self.panda_joint_num = len(self.panda_joint_names)
self.panda_state = [0.0] * self.panda_joint_num
# TF Listener
self.tf_listener = tf.TransformListener()
# TF2 Listener
# self.tf2_buffer = tf2_ros.Buffer()
# self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer)
# Static TF2 Broadcaster
# self.static_tf2_broadcaster = tf2_ros.StaticTransformBroadcaster()
# Robot control rate
self.sleep_time = (1.0 / rospy.get_param('~action_cycle_rate')) - 0.01
self.control_period = rospy.Duration.from_sec(self.sleep_time)
self.reference_frame = rospy.get_param('~reference_frame', 'base')
self.ee_frame = 'tool0' # TODO is the value for self.ee_frame correct?
self.target_frame = 'target'
# Minimum Trajectory Point time from start
# TODO check if this value is correct for the panda robot
self.min_traj_duration = 0.5
if not self.real_robot:
# Subscribers to link collision sensors topics
# TODO add rospy.Subscribers
rospy.Subscriber('/joint_states', JointState, self.callback_panda)
# TODO add keys to collision sensors
self.collision_sensors = dict.fromkeys([], False)
# TODO currently not used
self.safe_to_move = True
# Target mode
self.target_mode = rospy.get_param('~target_mode', FIXED_TARGET_MODE)
self.target_mode_name = rospy.get_param('~target_model_name', 'box100')
class PandaRosBridge:
def __init__(self, real_robot=False):
# Event is clear while initialization or set_state is going on
self.reset = Event()
self.reset.clear()
self.get_state_event = Event()
self.get_state_event.set()
self.real_robot = real_robot
# TODO publisher, subscriber, target and state
self._add_publishers()
self.panda_arm = ArmInterface()
self.target = [0.0] * 6 # TODO define number of target floats
# TODO define number of panda states (At least the number of joints)
self.panda_joint_names = [
'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4',
'panda_joint5', 'panda_joint6', 'panda_joint7'
]
self.panda_finger_names = [
'panda_finger_joint1', 'panda_finger_joint2'
]
self.panda_joint_num = len(self.panda_joint_names)
self.panda_state = [0.0] * self.panda_joint_num
# TF Listener
self.tf_listener = tf.TransformListener()
# TF2 Listener
# self.tf2_buffer = tf2_ros.Buffer()
# self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer)
# Static TF2 Broadcaster
# self.static_tf2_broadcaster = tf2_ros.StaticTransformBroadcaster()
# Robot control rate
self.sleep_time = (1.0 / rospy.get_param('~action_cycle_rate')) - 0.01
self.control_period = rospy.Duration.from_sec(self.sleep_time)
self.reference_frame = rospy.get_param('~reference_frame', 'base')
self.ee_frame = 'tool0' # TODO is the value for self.ee_frame correct?
self.target_frame = 'target'
# Minimum Trajectory Point time from start
# TODO check if this value is correct for the panda robot
self.min_traj_duration = 0.5
if not self.real_robot:
# Subscribers to link collision sensors topics
# TODO add rospy.Subscribers
rospy.Subscriber('/joint_states', JointState, self.callback_panda)
# TODO add keys to collision sensors
self.collision_sensors = dict.fromkeys([], False)
# TODO currently not used
self.safe_to_move = True
# Target mode
self.target_mode = rospy.get_param('~target_mode', FIXED_TARGET_MODE)
self.target_mode_name = rospy.get_param('~target_model_name', 'box100')
# Object parameters
# self.objects_controller = rospy.get_param('objects_controller', False)
# self.n_objects = int(rospy.get_param('n_objects', 0))
# if self.objects_controller:
# self.objects_model_name = []
# for i in range(self.n_objects):
# obj_model_name = rospy.get_param(
# 'object_' + repr(i) + '_model_name')
# self.objects_model_name.append(obj_model_name)
def callback_panda(self, data):
"""Callback function which sets the panda state
- `data.name` -> joint names
- `data.position` -> current joint positions
- `data.velocity` -> current velocity of joints
- `data.effort` -> current torque (effort) of joints
Args:
`data` (JointStates): data containing the current joint states
"""
# TODO gripper might also be included in this state
# if self.get_state_event.is_set():
self.panda_state[0:7] = data.position[0:7]
self.panda_state[7:14] = data.velocity[0:7]
self.panda_state[14:21] = data.effort[0:7]
def _add_publishers(self):
"""Adds publishers to ROS bridge
- joint trajectory command handler
- RViz target marker
"""
# joint_trajectory_command handler publisher
self.arm_cmd_pub = rospy.Publisher('env_arm_command',
JointTrajectory,
queue_size=1)
# Target RViz Marker publisher
self.target_pub = rospy.Publisher('target_marker',
Marker,
queue_size=10)
def get_state(self):
self.get_state_event.clear()
# # currently only working on a fixed target mode
if self.target_mode == FIXED_TARGET_MODE:
target = copy.deepcopy(self.target)
else:
raise ValueError
panda_state = copy.deepcopy(self.panda_state)
# # TODO is ee_to_base_transform value correctly loaded and set
# (position, quaternion) = self.tf_listener.lookupTransform(
# self.reference_frame)
# ee_to_base_transform = position + quaternion
# # TODO currently not needed
# # if self.real_robot:
# # panda_collision = False
# # else:
# # panda_collision = any(self.collision_sensors.values())
self.get_state_event.set()
# Create and fill State message
msg = robot_server_pb2.State()
msg.state.extend(target)
msg.state.extend(panda_state)
# msg.state.extend(ee_to_base_transform)
# msg.state.extend([panda_collision])
msg.success = True
return msg
def set_state(self, state_msg):
# Set environment state
state = state_msg.state
self.reset.clear()
# Set target internal value
if self.target_mode == FIXED_TARGET_MODE:
# TODO found out how many state values are needed for panda
self.target = copy.deepcopy(state[0:6])
# Publish Target Marker
self.publish_target_marker(self.target)
# TODO Broadcast target tf2
# TODO setup objects movement
# if self.objects_controller:
transformed_j_pos = self._transform_panda_list_to_dict(state[6:13])
reset_steps = int(15.0 / self.sleep_time)
for _ in range(reset_steps):
self.panda_arm.set_joint_positions(transformed_j_pos)
self.reset.set()
rospy.sleep(self.control_period)
return True
def publish_env_arm_cmd(self, position_cmd):
"""Publish environment JointTrajectory msg.
Publish JointTrajectory message to the env_command topic.
Args:
position_cmd (type): Description of parameter `positions`.
Returns:
type: Description of returned object.
"""
# if self.safe_to_move:
# msg = JointTrajectory()
# msg.header = Header()
# msg.joint_names = copy.deepcopy(self.panda_joint_names)
# msg.points = [JointTrajectoryPoint()]
# msg.points[0].positions = position_cmd
# duration = []
# for i in range(len(msg.joint_names)):
# TODO check if the index is in bounds
# !!! Be careful with panda_state index here
# pos = self.panda_state[i]
# cmd = position_cmd[i]
# max_vel = self.panda_joint_vel_limits[i]
# temp_duration = max(abs(cmd - pos) / max_vel, self.min_traj_duration)
# duration.append(temp_duration)
# msg.points[0].time_from_start = rospy.Duration.from_sec(max(duration))
# print(msg)
# self.arm_cmd_pub.publish(msg)
if self.safe_to_move:
transformed_j_pos = self._transform_panda_list_to_dict(
position_cmd[0:7])
self.panda_arm.set_joint_positions(transformed_j_pos)
rospy.sleep(self.control_period)
return position_cmd
else:
rospy.sleep(self.control_period)
return [0.0] * self.panda_joint_num
def get_model_state_pose(self, model_name, relative_entity_name=''):
# method used to retrieve model pose from gazebo simulation
rospy.wait_for_service('/gazebo/get_model_state')
try:
model_state = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
s = model_state(model_name, relative_entity_name)
pose = [s.pose.position.x, s.pose.position.y, s.pose.position.z, \
s.pose.orientation.x, s.pose.orientation.y, s.pose.orientation.z, s.pose.orientation.w]
return pose
except rospy.ServiceException as e:
print("Service call failed:" + e)
def get_link_state(self, link_name, reference_frame=''):
"""Method is used to retrieve link state from gazebo simulation
Args:
link_name (name of the link): [description]
reference_frame (str, optional): [description]. Defaults to ''.
Returns:
state of the link corresponding to the given name ->
[x_pos, y_pos, z_pos, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel]
"""
gazebo_get_link_state_service = '/gazebo/get_link_state'
rospy.wait_for_service(gazebo_get_link_state_service)
try:
link_state_srv = rospy.ServiceProxy(gazebo_get_link_state_service,
GetLinkState)
link_coordinates = link_state_srv(link_name,
reference_frame).link_state
link_pose, link_twist = link_coordinates.pose, link_coordinates.twist
x_pos = link_pose.position.x
y_pos = link_pose.position.y
z_pos = link_pose.position.z
orientation = PyKDL.Rotation.Quaternion(link_pose.orientation.x,
link_pose.orientation.y,
link_pose.orientation.z,
link_pose.orientation.w)
euler_orientation = orientation.GetRPY()
roll = euler_orientation[0]
pitch = euler_orientation[1]
yaw = euler_orientation[2]
x_vel = link_twist.linear.x
y_vel = link_twist.linear.y
z_vel = link_twist.linear.z
roll_vel = link_twist.angular.x
pitch_vel = link_twist.angular.y
yaw_vel = link_twist.angular.z
return x_pos, y_pos, z_pos, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel
except rospy.ServiceException as err:
error_message = 'Service call failed: ' + err
rospy.logerr(error_message)
print(error_message)
def publish_target_marker(self, target_pose):
t_marker = Marker()
t_marker.type = 1 # =>CUBE
t_marker.action = 0
t_marker.frame_locked = 1
t_marker.pose.position.x = target_pose[0]
t_marker.pose.position.y = target_pose[1]
t_marker.pose.position.z = target_pose[2]
rpy_orientation = PyKDL.Rotation.RPY(target_pose[3], target_pose[4],
target_pose[5])
q_orientation = rpy_orientation.GetQuaternion()
t_marker.pose.orientation.x = q_orientation[0]
t_marker.pose.orientation.y = q_orientation[1]
t_marker.pose.orientation.z = q_orientation[2]
t_marker.pose.orientation.w = q_orientation[3]
t_marker.scale.x = 0.1
t_marker.scale.y = 0.1
t_marker.scale.z = 0.1
t_marker.id = 0
t_marker.header.stamp = rospy.Time.now()
t_marker.header.frame_id = self.reference_frame
t_marker.color.a = 0.7
t_marker.color.r = 1.0 # red
t_marker.color.g = 0.0
t_marker.color.b = 0.0
self.target_pub.publish(t_marker)
def _transform_panda_list_to_dict(self, panda_list):
transformed_dict = {}
for idx, value in enumerate(panda_list):
current_joint_name = self.panda_joint_names[idx]
transformed_dict[current_joint_name] = value
return transformed_dict
plt.xlabel("number of iterations (adding up to 10 seconds)")
plt.legend()
plt.subplot(122)
plt.title("Deviations from desired pose")
plt.plot(pose_error[0,10:], label = "deviation x [m]")
plt.plot(pose_error[1,10:], label = "deviation y [m]")
plt.plot(pose_error[2,10:], label = "deviation z [m]")
plt.plot(pose_error[3,10:], label = "quaternion x")
plt.plot(pose_error[4,10:], label = "quaternion y")
plt.plot(pose_error[5,10:], label = "quaternion z")
plt.xlabel("number of iterations (adding up to 10 seconds)")
plt.legend()
plt.show()
"""
if __name__ == "__main__":
rospy.init_node("my_very_own_node")
#rospy.on_shutdown(_on_shutdown)
publish_rate = 500
rate = rospy.Rate(publish_rate)
robot = ArmInterface()
print('Moving to starting position')
#robot.move_to_joint_positions(starting_position)
robot.move_to_neutral()
impedance_control(rate, K_Pt, K_Po, K_m, K_d)
return arm.convertToDict(dofs)
def randomQ(arm):
(lower, upper) = arm.GetJointLimits()
dofs = numpy.zeros(len(lower))
lower[0] = -math.pi / 2.0
upper[0] = math.pi / 2.0
for i in xrange(len(lower)):
dofs[i] = random.uniform(lower[i], upper[i])
return dofs
if __name__ == '__main__':
rospy.init_node("path_testing")
realarm = ArmInterface()
q0 = realarm.convertToList(realarm.joint_angles())
startq = realarm.joint_angles()
while True:
# Rather than peturbing pose, perturb q as a hack
delta_q = numpy.random.rand(7) * 0.2
q1_seed = numpy.add(q0, delta_q)
raw_input("seed")
realarm.move_to_joint_positions(realarm.convertToDict(q1_seed))
p1 = realarm.endpoint_pose()
raw_input("q0")
realarm.move_to_joint_positions(realarm.convertToDict(q0))
p0 = realarm.endpoint_pose()
def ExecuteActions(plan,
real=False,
pause=True,
wait=True,
prompt=True,
obstacles=[],
sim_fatal_failure_prob=0,
sim_recoverable_failure_prob=0):
# if prompt:
# input("Execute in Simulation?")
# obj_held = None
for name, args in plan:
# pb_robot.viz.remove_all_debug()
# bodyNames = [args[i].get_name() for i in range(len(args)) if isinstance(args[i], pb_robot.body.Body)]
#txt = '{} - {}'.format(name, bodyNames)
# pb_robot.viz.add_text(txt, position=(0, 0.25, 0.5), size=2)
executionItems = args[-1]
for e in executionItems:
if real:
e.simulate(timestep=0.1, obstacles=obstacles)
else:
e.simulate(timestep=0.05, obstacles=obstacles)
# Assign the object being held
# if isinstance(e, pb_robot.vobj.BodyGrasp):
# if name == "pick":
# obj_held = e
# else:
# obj_held = None
# Simulate failures if specified
if (name in ["pick", "move_free"]
and not isinstance(e, pb_robot.vobj.BodyGrasp)
and not isinstance(e, pb_robot.vobj.MoveFromTouch)):
if numpy.random.rand() < sim_fatal_failure_prob:
raise ExecutionFailure(
fatal=True, reason=f"Simulated fatal failure in {e}")
elif numpy.random.rand() < sim_recoverable_failure_prob:
# if (name in ["place", "place_home", "move_holding"]) or \
# (name=="pick" and isinstance(e, pb_robot.vobj.MoveFromTouch)):
# obj_held_arg = obj_held
# else:
# obj_held_arg = None
raise ExecutionFailure(
fatal=False,
reason=f"Simulated recoverable failure in {e}")
if wait:
input("Next?")
elif pause:
time.sleep(0.1)
if real:
input("Execute on Robot?")
try:
from franka_interface import ArmInterface
except:
print("Do not have rospy and franka_interface installed.")
return
#try:
arm = ArmInterface()
arm.set_joint_position_speed(0.3)
#except:
# print("Unable to connect to real robot. Exiting")
# return
print("Executing on real robot")
input("start?")
for name, args in plan:
executionItems = args[-1]
for e in executionItems:
e.execute(realRobot=arm, obstacles=obstacles)
import rospy
from franka_interface import ArmInterface
if __name__ == '__main__':
rospy.init_node("test_robot")
r = ArmInterface()
cm = r.get_controller_manager()
# **************************************************************************/
# /***************************************************************************
# Copyright (c) 2019, Saif Sidhik
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
# http://www.apache.org/licenses/LICENSE-2.0
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# **************************************************************************/
"""
@info:
commands robot to move to neutral pose
"""
import rospy
from franka_interface import ArmInterface
if __name__ == '__main__':
rospy.init_node("move_to_neutral_node")
r = ArmInterface()
r.move_to_neutral()
import rospy
import numpy as np
from franka_interface import ArmInterface
"""
:info:
Demo showing ways to use API. Also shows how to move the robot using low-level controllers.
WARNING: The robot will move slightly (small arc swinging motion side-to-side) till code is killed.
"""
if __name__ == '__main__':
rospy.init_node("test_robot")
r = ArmInterface(
) # create arm interface instance (see https://justagist.github.io/franka_ros_interface/DOC.html#arminterface for all available methods for ArmInterface() object)
cm = r.get_controller_manager(
) # get controller manager instance associated with the robot (not required in most cases)
mvt = r.get_movegroup_interface(
) # get the moveit interface for planning and executing trajectories using moveit planners (see https://justagist.github.io/franka_ros_interface/DOC.html#franka_moveit.PandaMoveGroupInterface for documentation)
elapsed_time_ = rospy.Duration(0.0)
period = rospy.Duration(0.005)
r.move_to_neutral() # move robot to neutral pose
initial_pose = r.joint_angles() # get current joint angles of the robot
jac = r.zero_jacobian() # get end-effector jacobian
count = 0
rate = rospy.Rate(1000)
import rospy
from franka_interface import ArmInterface
import numpy as np
# import matplotlib.pyplot as plt
# from std_msgs.msg import Float64
from copy import deepcopy
names = [
'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4',
'panda_joint5', 'panda_joint6', 'panda_joint7'
]
if __name__ == '__main__':
rospy.init_node("test_node")
r = ArmInterface()
rate = rospy.Rate(400)
elapsed_time_ = rospy.Duration(0.0)
period = rospy.Duration(0.005)
r.move_to_neutral() # move to neutral pose before beginning
initial_pose = deepcopy(r.joint_ordered_angles())
raw_input("Hit Enter to Start")
print "commanding"
vals = deepcopy(initial_pose)
count = 0
global visual_features
visual_features = msg
def skew_mat(p):
return np.array([
[0, -p[2], p[1]],
[p[2], 0, -p[0]],
[-p[1], p[0], 0]
])
if __name__ == '__main__':
rospy.init_node("reference_governor")
# Create the robot
robot = ArmInterface()
vals_pos = robot.joint_angles()
vals_vel = robot.joint_velocities()
joint_names = robot.joint_names()
rospy.loginfo('RefGov: Moving to neutral')
robot.move_to_neutral()
# Subscriber to the camera_info topic
camera_info_msg = rospy.wait_for_message("/d435_color/camera_info", CameraInfo, timeout=None)
intrinsic = get_intrinsic_param(camera_info_msg)
# Transformations
tf_listener = tf.TransformListener()
#! /usr/bin/env python
"""
@info:
commands robot to move to neutral pose
"""
import rospy
import copy
import IPython
import numpy
from franka_interface import ArmInterface
if __name__ == '__main__':
rospy.init_node("path_testing")
arm = ArmInterface()
frames = arm.get_frames_interface()
start_pose = arm.endpoint_pose()
pose0 = copy.deepcopy(start_pose)
pose1 = copy.deepcopy(pose0)
pose1['position'][0] += 0.1
pose2 = copy.deepcopy(pose1)
pose2['position'][1] += 0.1
pose3 = copy.deepcopy(pose2)
pose3['position'][0] -= 0.1
pose4 = copy.deepcopy(pose3)
pose4['position'][1] -= 0.1
path = [pose1, pose2, pose3, pose4]
# Stop recording rosbag
terminate_ros_node("/record")
rospy.sleep(1)
#hand.Open() TODO
def zeroFTSensor():
zeroSensorRos = rospy.ServiceProxy('/netft/zero', srv.Zero)
rospy.wait_for_service('/netft/zero', timeout=0.5)
zeroSensorRos()
def terminate_ros_node(s):
list_cmd = subprocess.Popen("rosnode list",
shell=True,
stdout=subprocess.PIPE)
list_output = list_cmd.stdout.read()
retcode = list_cmd.wait()
assert retcode == 0, "List command returned %d" % retcode
for string in list_output.split("\n"):
if string.startswith(s):
os.system("rosnode kill " + string)
if __name__ == '__main__':
rospy.init_node("grip_tests")
realarm = ArmInterface()
q0 = realarm.joint_angles()
IPython.embed()
plt.plot(pose_error[1,10:], label = "deviation y [m]")
plt.plot(pose_error[2,10:], label = "deviation z [m]")
plt.plot(pose_error[3,10:], label = "quaternion x")
plt.plot(pose_error[4,10:], label = "quaternion y")
plt.plot(pose_error[5,10:], label = "quaternion z")
plt.xlabel("number of iterations (adding up to 10 seconds)")
plt.legend()
plt.show()
if __name__ == "__main__":
rospy.init_node("ts_control_sim_only")
#rospy.on_shutdown(_on_shutdown)
publish_rate = 500
rate = rospy.Rate(publish_rate)
robot = ArmInterface()
print('Moving to starting position')
robot.move_to_joint_positions(new_start)
#robot.move_to_neutral()
impedance_control(rate,K_Pt,K_Po,K_m,K_d)
assert self._current_K_frame_transformation is not None, "FrankaFramesInterface: Current K Frame is not known."
return list(self._current_K_frame_transformation) == list(
DEFAULT_TRANSFORMATIONS.K_FRAME)
def _request_setK_service(self, trans_mat):
print trans_mat
rospy.wait_for_service(
'/franka_ros_interface/franka_control/set_K_frame')
try:
service_handle = rospy.ServiceProxy(
'/franka_ros_interface/franka_control/set_K_frame', SetKFrame)
response = service_handle(EE_T_K=trans_mat)
rospy.loginfo(
"Set K Frame Request Status: %s. \n\tDetails: %s" %
("Success" if response.success else "Failed!", response.error))
return response.success
except rospy.ServiceException, e:
rospy.logwarn("Set K Frame Request: Service call failed: %s" % e)
return False
if __name__ == '__main__':
# main()
from franka_interface import ArmInterface
rospy.init_node("test")
ee_setter = FrankaFramesInterface(ArmInterface())
# ee_setter.set_EE_frame([1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1])
"""
global ctrl_thread
if ctrl_thread.is_alive():
ctrl_thread.join()
if __name__ == "__main__":
# global goal_pos, goal_ori, ctrl_thread
rospy.init_node("ts_control_sim_only")
# when using franka_ros_interface, the robot can be controlled through and
# the robot state is directly accessible with the API
# If using the panda_robot API, this will be
# panda = PandaArm()
robot = ArmInterface()
# when using the panda_robot interface, the next 2 lines can be simplified
# to: `start_pos, start_ori = panda.ee_pose()`
ee_pose = robot.endpoint_pose()
start_pos, start_ori = ee_pose['position'], ee_pose['orientation']
goal_pos, goal_ori = start_pos, start_ori
# start controller thread
rospy.on_shutdown(_on_shutdown)
rate = rospy.Rate(publish_rate)
ctrl_thread = threading.Thread(target=control_thread, args = [rate])
ctrl_thread.start()
# ------------------------------------------------------------------------------------
This code is for testing the torque controller. The robot should be in zeroG mode, i.e.
It should stay still when no external force is acting, but should be very compliant
(almost no resistance) when pushed. This code sends zero torque command continuously,
which means the internal gravity compensation is the only torque being sent to the
robot.
Test by pushing robot.
DO NOT RUN THIS CODE WITH CUSTOM END-EFFECTOR UNLESS YOU KNOW WHAT YOU'RE DOING!
WARNING: This code only works if the robot model is good! If you have installed custom
end-effector, the gravity compensation may not be good unless you have incorporated
the model to the FCI via Desk!!
"""
if __name__ == '__main__':
rospy.init_node("test_zeroG")
r = ArmInterface() # create arm interface instance (see https://justagist.github.io/franka_ros_interface/DOC.html#arminterface for all available methods for ArmInterface() object)
rospy.loginfo("Commanding...\n")
r.move_to_neutral() # move robot to neutral pose
rate = rospy.Rate(1000)
joint_names = r.joint_names()
while not rospy.is_shutdown():
r.set_joint_torques(dict(zip(joint_names, [0.0]*7))) # send 0 torques
rate.sleep()
def map_keyboard():
"""
Map keyboard keys to robot joint motion. Keybindings can be
found when running the script.
"""
limb = ArmInterface()
gripper = GripperInterface(ns=limb.get_robot_params().get_base_namespace())
has_gripper = gripper.exists
joints = limb.joint_names()
def set_j(limb, joint_name, delta):
joint_command = limb.joint_angles()
joint_command[joint_name] += delta
limb.set_joint_positions(joint_command)
# limb.set_joint_positions_velocities([joint_command[j] for j in joints], [0.00001]*7) # impedance control when using this example might fail because commands are sent too quickly for the robot to respond
def set_g(action):
if has_gripper:
if action == "close":
gripper.close()
elif action == "open":
gripper.open()
elif action == "calibrate":
gripper.calibrate()
bindings = {
'1': (set_j, [limb, joints[0], 0.01], joints[0] + " increase"),
'q': (set_j, [limb, joints[0], -0.01], joints[0] + " decrease"),
'2': (set_j, [limb, joints[1], 0.01], joints[1] + " increase"),
'w': (set_j, [limb, joints[1], -0.01], joints[1] + " decrease"),
'3': (set_j, [limb, joints[2], 0.01], joints[2] + " increase"),
'e': (set_j, [limb, joints[2], -0.01], joints[2] + " decrease"),
'4': (set_j, [limb, joints[3], 0.01], joints[3] + " increase"),
'r': (set_j, [limb, joints[3], -0.01], joints[3] + " decrease"),
'5': (set_j, [limb, joints[4], 0.01], joints[4] + " increase"),
't': (set_j, [limb, joints[4], -0.01], joints[4] + " decrease"),
'6': (set_j, [limb, joints[5], 0.01], joints[5] + " increase"),
'y': (set_j, [limb, joints[5], -0.01], joints[5] + " decrease"),
'7': (set_j, [limb, joints[6], 0.01], joints[6] + " increase"),
'u': (set_j, [limb, joints[6], -0.01], joints[6] + " decrease")
}
if has_gripper:
bindings.update({
'8': (set_g, "close", "close gripper"),
'i': (set_g, "open", "open gripper"),
'9': (set_g, "calibrate", "calibrate gripper")
})
done = False
rospy.logwarn(
"Controlling joints. Press ? for help, Esc to quit.\n\nWARNING: The motion will be slightly jerky!!\n"
)
while not done and not rospy.is_shutdown():
c = getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
elif c in bindings:
cmd = bindings[c]
if c == '8' or c == 'i' or c == '9':
cmd[0](cmd[1])
print("command: %s" % (cmd[2], ))
else:
#expand binding to something like "set_j(right, 'j0', 0.1)"
cmd[0](*cmd[1])
print("command: %s" % (cmd[2], ))
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(viewitems(bindings),
key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
plt.plot(pose_error[1,10:], label = "deviation y [m]")
plt.plot(pose_error[2,10:], label = "deviation z [m]")
plt.plot(pose_error[3,10:], label = "quaternion x")
plt.plot(pose_error[4,10:], label = "quaternion y")
plt.plot(pose_error[5,10:], label = "quaternion z")
plt.xlabel("number of iterations (adding up to 10 seconds)")
plt.legend()
plt.show()
"""
if __name__ == "__main__":
rospy.init_node("my_very_own_node")
#rospy.on_shutdown(_on_shutdown)
publish_rate = 500
rate = rospy.Rate(publish_rate)
robot = ArmInterface()
print('Moving to starting position')
robot.move_to_joint_positions(next_to_dummy)
#robot.move_to_neutral()
impedance_control(rate,K_Pt,K_Po,K_m,K_d)
from franka_interface import ArmInterface
import numpy as np
from builtins import input
# import matplotlib.pyplot as plt
# from std_msgs.msg import Float64
from copy import deepcopy
names = [
'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4',
'panda_joint5', 'panda_joint6', 'panda_joint7'
]
if __name__ == '__main__':
rospy.init_node("test_node")
r = ArmInterface()
rate = rospy.Rate(400)
elapsed_time_ = rospy.Duration(0.0)
period = rospy.Duration(0.005)
r.move_to_neutral() # move to neutral pose before beginning
initial_pose = deepcopy(r.joint_ordered_angles())
input("Hit Enter to Start")
print("commanding")
vals = deepcopy(initial_pose)
count = 0
plt.plot(sensor_readings[5, 10:], label="torque z [Nm]")
plt.xlabel("number of iterations (adding up to 10 seconds)")
plt.legend()
plt.subplot(122)
plt.title("Deviations from desired pose")
plt.plot(pose_error[0, 10:], label="deviation x [m]")
plt.plot(pose_error[1, 10:], label="deviation y [m]")
plt.plot(pose_error[2, 10:], label="deviation z [m]")
plt.plot(pose_error[3, 10:], label="quaternion x")
plt.plot(pose_error[4, 10:], label="quaternion y")
plt.plot(pose_error[5, 10:], label="quaternion z")
plt.xlabel("number of iterations (adding up to 10 seconds)")
plt.legend()
plt.show()
if __name__ == "__main__":
rospy.init_node("ts_control_sim_only")
#rospy.on_shutdown(_on_shutdown)
publish_rate = 500
rate = rospy.Rate(publish_rate)
robot = ArmInterface()
print('Moving to starting position')
robot.move_to_joint_positions(starting_position)
#robot.move_to_neutral()
impedance_control(rate, K_Pt, K_Po, K_m, K_d)
def main(args):
NOISE = 0.00005
# get a bunch of random blocks
blocks = load_blocks(fname=args.blocks_file, num_blocks=10, remove_ixs=[1])
agent = PandaAgent(blocks,
NOISE,
use_platform=False,
teleport=False,
use_action_server=False,
use_vision=False,
real=True)
agent.step_simulation(T=1, vis_frames=True, lifeTime=0.)
agent.plan()
fixed = [f for f in agent.fixed if f is not None]
grasps_fn = get_grasp_gen(agent.robot,
add_slanted_grasps=False,
add_orthogonal_grasps=True)
path_planner = get_free_motion_gen(agent.robot, fixed)
ik_fn = get_ik_fn(agent.robot,
fixed,
approach_frame='gripper',
backoff_frame='global',
use_wrist_camera=False)
from franka_interface import ArmInterface
arm = ArmInterface()
#arm.move_to_neutral()
start_q = arm.convertToList(arm.joint_angles())
start_q = pb_robot.vobj.BodyConf(agent.robot, start_q)
body = agent.pddl_blocks[args.id]
pose = pb_robot.vobj.BodyPose(body, body.get_base_link_pose())
for g in grasps_fn(body):
grasp = g[0]
# Check that the grasp points straight down.
obj_worldF = pb_robot.geometry.tform_from_pose(pose.pose)
grasp_worldF = np.dot(obj_worldF, grasp.grasp_objF)
grasp_worldR = grasp_worldF[:3, :3]
e_x, e_y, e_z = np.eye(3)
is_top_grasp = grasp_worldR[:, 2].dot(-e_z) > 0.999
if not is_top_grasp: continue
print('Getting IK...')
approach_q = ik_fn(body, pose, grasp, return_grasp_q=True)[0]
print('Planning move to path')
command1 = path_planner(start_q, approach_q)
print('Planning return home')
command2 = path_planner(approach_q, start_q)
agent.execute()
input('Ready to execute?')
command1[0][0].simulate(timestep=0.25)
input('Move back in sim?')
command2[0][0].simulate(timestep=0.25)
input('Move to position on real robot?')
command1[0][0].execute(realRobot=arm)
input('Reset position on real robot?')
command2[0][0].execute(realRobot=arm)
break
