def _pub_joint_torques(self, torques):
joint_state = JointState()
joint_state.effort = tuple(torques)
self.joint_pub.publish(joint_state)
delta_q = jacobian_inv * delta_x
q_1_delta = delta_q[0][0]
q_2_delta = delta_q[1][0]
q_3_delta = delta_q[2][0]
self.q_1 = self.q_1 + q_1_delta
self.q_2 = self.q_2 + q_2_delta
self.q_3 = self.q_3 + q_3_delta
rf = quad_leg(-a_global, -b_global, q_1_nom, q_2_nom, q_3_nom, l_1_global)
lf = quad_leg(a_global, -b_global, q_1_nom, q_2_nom, q_3_nom, -l_1_global)
rb = quad_leg(-a_global, -b_global, q_1_nom, q_2_nom, q_3_nom, l_1_global)
lb = quad_leg(a_global, -b_global, q_1_nom, q_2_nom, q_3_nom, -l_1_global)
hello_str = JointState()
hello_str.header = Header()
hello_str.name = [
'shoulder_joint_lf', 'elbow_joint_lf', 'wrist_joint_lf', 'ankle_joint_lf',
'shoe_joint_lf', 'shoulder_joint_rf', 'elbow_joint_rf', 'wrist_joint_rf',
'ankle_joint_rf', 'shoe_joint_rf', 'shoulder_joint_lb', 'elbow_joint_lb',
'wrist_joint_lb', 'fffankle_joint_lb', 'shoe_joint_lb',
'shoulder_joint_rb', 'elbow_joint_rb', 'wrist_joint_rb', 'ankle_joint_rb',
'shoe_joint_rb'
]
hello_str.velocity = []
hello_str.effort = []
def callback(data):
rf.set_nom_position()
def joint_publish(self, q):
joint_command = JointState()
joint_command.header.stamp = rospy.Time.now()
joint_command.name = ['joint1', 'joint2']
joint_command.position = q
self.joint_pub.publish(joint_command)
def __init__(self,
prefix="",
gripper="",
interface='eth0',
jaco_ip="10.66.171.15",
dof=""):
"""
Constructor
"""
# Setup a lock for accessing data in the control loop
self._lock = threading.Lock()
# Assume success until posted otherwise
rospy.loginfo('Starting JACO2 control')
self.init_success = True
self._prefix = prefix
self.iface = interface
self.arm_dof = dof
# List of joint names
if ("6dof" == self.arm_dof):
self._joint_names = [
self._prefix + '_shoulder_pan_joint',
self._prefix + '_shoulder_lift_joint',
self._prefix + '_elbow_joint', self._prefix + '_wrist_1_joint',
self._prefix + '_wrist_2_joint',
self._prefix + '_wrist_3_joint'
]
elif ("7dof" == self.arm_dof):
self._joint_names = [
self._prefix + '_shoulder_pan_joint',
self._prefix + '_shoulder_lift_joint',
self._prefix + '_arm_half_joint',
self._prefix + '_elbow_joint',
self._prefix + '_wrist_spherical_1_joint',
self._prefix + '_wrist_spherical_2_joint',
self._prefix + '_wrist_3_joint'
]
else:
rospy.logerr(
"DoF needs to be set 6 or 7, cannot start SIArmController")
return
self._num_joints = len(self._joint_names)
# Create the hooks for the API
if ('left' == prefix):
self.api = KinovaAPI('left', self.iface, jaco_ip, '255.255.255.0',
24000, 24024, 44000, self.arm_dof)
elif ('right' == prefix):
self.api = KinovaAPI('right', self.iface, jaco_ip, '255.255.255.0',
25000, 25025, 55000, self.arm_dof)
else:
rospy.logerr(
"prefix needs to be set to left or right, cannot start the controller"
)
return
if not (self.api.init_success):
self.Stop()
return
self.api.SetCartesianControl()
self._position_hold = False
self.estop = False
# Initialize the joint feedback
pos = self.api.get_angular_position()
vel = self.api.get_angular_velocity()
force = self.api.get_angular_force()
self._joint_fb = dict()
self._joint_fb['position'] = pos[:self._num_joints]
self._joint_fb['velocity'] = vel[:self._num_joints]
self._joint_fb['force'] = force[:self._num_joints]
if ("kg2" == gripper) or ("rq85" == gripper):
self._gripper_joint_names = [
self._prefix + '_gripper_finger1_joint',
self._prefix + '_gripper_finger2_joint'
]
self.num_fingers = 2
elif ("kg3" == gripper):
self._gripper_joint_names = [
self._prefix + '_gripper_finger1_joint',
self._prefix + '_gripper_finger2_joint',
self._prefix + '_gripper_finger3_joint'
]
self.num_fingers = 3
if (0 != self.num_fingers):
self._gripper_fb = dict()
self._gripper_fb['position'] = pos[self.
_num_joints:self._num_joints +
self.num_fingers]
self._gripper_fb['velocity'] = vel[self.
_num_joints:self._num_joints +
self.num_fingers]
self._gripper_fb['force'] = force[self.
_num_joints:self._num_joints +
self.num_fingers]
# Register the publishers and subscribers
self.last_cartesian_vel_cmd_update = rospy.get_time() - 0.5
# X, Y, Z, ThetaX, ThetaY, ThetaZ, FingerVel
self._last_cartesian_vel_cmd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self._cartesian_vel_cmd_sub = rospy.Subscriber(
"/movo/%s_arm/cartesian_vel_cmd" % self._prefix,
JacoCartesianVelocityCmd, self._update_cartesian_vel_cmd)
self.last_angular_vel_cmd_update = rospy.get_time() - 0.5
self._last_angular_vel_cmd = [0.0
] * (self._num_joints + self.num_fingers)
if "6dof" == self.arm_dof:
self._angular_vel_cmd_sub = rospy.Subscriber(
"/movo/%s_arm/angular_vel_cmd" % self._prefix,
JacoAngularVelocityCmd6DOF, self._update_angular_vel_cmd)
elif "7dof" == self.arm_dof:
self._angular_vel_cmd_sub = rospy.Subscriber(
"/movo/%s_arm/angular_vel_cmd" % self._prefix,
JacoAngularVelocityCmd7DOF, self._update_angular_vel_cmd)
else:
# Error condition
rospy.logerr("DoF needs to be set 6 or 7, was {}".format(
self.arm_dof))
self.Stop()
return
self._gripper_vel_cmd = 0.0
self._ctl_mode = SIArmController.TRAJECTORY
self.api.set_control_mode(KinovaAPI.ANGULAR_CONTROL)
self._jstpub = rospy.Publisher("/movo/%s_arm_controller/state" %
self._prefix,
JointTrajectoryControllerState,
queue_size=10)
self._jstmsg = JointTrajectoryControllerState()
self._jstmsg.header.seq = 0
self._jstmsg.header.frame_id = ''
self._jstmsg.header.stamp = rospy.get_rostime()
self._jspub = rospy.Publisher("/movo/%s_arm/joint_states" %
self._prefix,
JointState,
queue_size=10)
self._jsmsg = JointState()
self._jsmsg.header.seq = 0
self._jsmsg.header.frame_id = ''
self._jsmsg.header.stamp = rospy.get_rostime()
self._jsmsg.name = self._joint_names
self._actfdbk_pub = rospy.Publisher("/movo/%s_arm/actuator_feedback" %
self._prefix,
KinovaActuatorFdbk,
queue_size=10)
self._actfdbk_msg = KinovaActuatorFdbk()
self._jsmsg.header.seq = 0
self._jsmsg.header.frame_id = ''
self._jsmsg.header.stamp = rospy.get_rostime()
if (0 != self.num_fingers):
self._gripper_vel_cmd_sub = rospy.Subscriber(
"/movo/%s_gripper/vel_cmd" % self._prefix, Float32,
self._update_gripper_vel_cmd)
self._gripper_jspub = rospy.Publisher(
"/movo/%s_gripper/joint_states" % self._prefix,
JointState,
queue_size=10)
self._gripper_jsmsg = JointState()
self._gripper_jsmsg.header.seq = 0
self._gripper_jsmsg.header.frame_id = ''
self._gripper_jsmsg.header.stamp = rospy.get_rostime()
self._gripper_jsmsg.name = self._gripper_joint_names
# Set initial parameters for the controller
if (0 != self.num_fingers):
self._gripper_pid = [None] * self.num_fingers
for i in range(self.num_fingers):
self._gripper_pid[i] = JacoPID(5.0, 0.0, 0.8)
self._gripper_vff = DifferentiateSignals(
self.num_fingers, self._gripper_fb['position'])
self._gripper_rate_limit = RateLimitSignals(
[FINGER_ANGULAR_VEL_LIMIT] * self.num_fingers,
self.num_fingers, self._gripper_fb['position'])
if ("6dof" == self.arm_dof):
self._arm_rate_limit = RateLimitSignals(JOINT_6DOF_VEL_LIMITS,
self._num_joints,
self._joint_fb['position'])
if ("7dof" == self.arm_dof):
self._arm_rate_limit = RateLimitSignals(JOINT_7DOF_VEL_LIMITS,
self._num_joints,
self._joint_fb['position'])
self._arm_vff_diff = DifferentiateSignals(self._num_joints,
self._joint_fb['position'])
self._pid = [None] * self._num_joints
for i in range(self._num_joints):
self._pid[i] = JacoPID(5.0, 0.0, 0.8)
self.pause_controller = False
self._init_ext_joint_position_control()
self._init_ext_gripper_control()
# Update the feedback once to get things initialized
self._update_controller_data()
# Start the controller
rospy.loginfo("Starting the %s controller" % self._prefix)
self._done = False
self._t1 = rospy.Timer(rospy.Duration(0.01), self._run_ctl)
def __init__(self):
rospy.init_node('arm_tracker')
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.on_shutdown(self.shutdown)
# Initialize the move group for the right arm
self.right_arm = moveit_commander.MoveGroupCommander(GROUP_NAME_ARM)
self.right_arm.set_planner_id("RRTkConfigDefault")
# Keep track of the last target pose
self.last_target_pose = PoseStamped()
# Set the right arm reference frame
self.right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allowing replanning slows things down
self.right_arm.allow_replanning(False)
# Set a position tolerance in meters
self.right_arm.set_goal_position_tolerance(0.01)
# Set an orientation tolerance in radians
self.right_arm.set_goal_orientation_tolerance(0.05)
self.right_arm.set_planning_time(0.02)
# Set max acceleration and velocity scaling (0-1)
self.right_arm.set_max_acceleration_scaling_factor(1.0)
self.right_arm.set_max_velocity_scaling_factor(1.0)
# What is the end effector link?
self.end_effector_link = self.right_arm.get_end_effector_link()
# Track how many times are given a request
self.request_count = 0
# Track how many requests are successful
self.move_count = 0
# Use Cartesian planning?
self.cartesian = True
# How close is close enough? (Radians for joint angles)
self.close_enough = 0.01
#self.joint_names = self.right_arm.get_joint_names()
# Monitor the current joint states
self.joint_states = JointState()
rospy.wait_for_message('joint_states', JointState)
self.joint_states_subscriber = rospy.Subscriber(
'joint_states', JointState, self.update_joint_states, queue_size=1)
# Use queue_size=1 so we don't pile up outdated target messages
self.target_subscriber = rospy.Subscriber('target_pose',
PoseStamped,
self.update_target_pose,
queue_size=1)
rospy.loginfo("Ready for action!")
def initialize(self):
self.msg = JointState()
return True
def update_simulation(world, sim, d_time):
# This code manually updates the visualization while doing other stuff for ROS: publishing joint_states, floating
# frame's tf, publishing touch data, object pose and twist
# Creating the present robot
present_robot = world.robot(world.numRobots() - 1)
# Creating the joints name array
joint_names_array = []
for i in joints_dict:
joint_names_array.append(joints_dict[i])
# Creating the JointState msg to be published
joints_msg = JointState()
joints_msg.name = joint_names_array
joints_msg.effort = []
joints_msg.velocity = []
# Creating a TransformStamped
static_transform_stamped = geometry_msgs.msg.TransformStamped()
static_transform_stamped.header.frame_id = world_frame_name
# Creating the Int8 msg for touch publishing and the touch memory
touch_msg = Int8()
touch_memory = []
vis.add("world", world)
vis.show()
t0 = time.time()
sim_time = d_time
print 'The sim time in main is ', d_time
# Publishing all what is needed for adaptive grasping and calling it
publish_joints(present_robot, joints_msg)
publish_palm(present_robot, static_transform_stamped)
publish_object(world, static_transform_stamped)
# Waiting and calling the adaptive grasping
rospy.wait_for_service(adaptive_grasping_service_name)
if not call_adaptive_grasping(True):
return
# Waiting for a message in the hand and palm command topics
# This is to avoid the integration in adaptive_controller crashing
rospy.wait_for_message(hand_topic, Float64, 3.0)
rospy.wait_for_message(arm_topic, Twist, 3.0)
while vis.shown():
# Simulating and updating visualization
vis.lock()
sim.simulate(sim_time)
sim.updateWorld()
vis.unlock()
# Publishing joint states, palm frame and object frame, pose and twist
publish_joints(present_robot, joints_msg)
publish_palm(present_robot, static_transform_stamped)
publish_object(world, static_transform_stamped)
# Getting the new touch id and publishing it while checking for algorithm stopping conditions
force_palm, num_contacts = check_contacts(world, sim, touch_memory,
touch_msg)
syn_now = sim.controller(0).getSensedConfig()[34]
# Stopping adaptive grasping if stopping conditions (too much force on palm or too many contacts)
print 'The stop adaptive bool is ', global_vars.stop_adaptive_command
if force_palm or num_contacts >= max_num_contacts or syn_now > global_vars.syn_closed or global_vars.stop_adaptive_command.data:
if global_vars.is_adaptive_running:
if not call_adaptive_grasping(False):
return
# Sleeping a little bit
t1 = time.time()
time.sleep(max(0.01 - (t1 - t0), 0.001))
t0 = t1
return
def __init__(self, use_moveit=False):
# self.is_stopping = False
rospy.loginfo('Starting the Franka interface!')
rospy.init_node("deeprobot_franka_interface")
# self.rate = rospy.Rate(30) # 30 Hz
self.rate = rospy.Rate(100) # 100 Hz
self.curr_velo_pub = rospy.Publisher(
'/joint_velocity_node_controller/joint_velocity',
JointVelocity,
queue_size=1)
self.curr_pos_pub = rospy.Publisher(
'/joint_position_node_controller/joint_position',
JointPosition,
queue_size=1)
# self.pc = PandaCommander(group_name='panda_arm_hand')
self.joint_names = {
'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1',
'left_w2'
}
self.index_corr = {
'left_s0': 1,
'left_s1': 2,
'left_e0': 3,
'left_e1': 4,
'left_w0': 5,
'left_w1': 6,
'left_w2': 7
} # the index correspondence
self.bridge = cv_bridge.CvBridge()
self.curr_im = np.zeros((3, 480, 640))
self.control_accuracy = 0.01
self.use_moveit = use_moveit
if self.use_moveit:
self.cs = ControlSwitcher({
'moveit':
'position_joint_trajectory_controller',
'velocity':
'joint_velocity_node_controller'
})
rospy.wait_for_message('move_group/status', GoalStatusArray)
self.panda_moveit_commander = MoveGroupCommander('panda_arm')
else:
self.cs = ControlSwitcher({
'position':
'joint_position_node_controller',
'velocity':
'joint_velocity_node_controller'
})
self.cs.switch_controller('velocity')
self.target_joint_state = JointState()
self.target_joint_state.name = [
"panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4",
"panda_joint5", "panda_joint6", "panda_joint7"
]
self.target_joint_state.position = [
-1.29, -0.26, -0.27, -2.34, 0.12, 2.10, 0.48
]
self.robot_state = None
self.curr_joint_angles = None
self.ROBOT_ERROR_DETECTED = False
self.image_sub = rospy.Subscriber("/camera/color/image_raw", Image,
self.__updateCurrRGBImage)
self.robot_state_sub = rospy.Subscriber(
'/franka_state_controller/franka_states',
FrankaState,
self.__robot_state_callback,
queue_size=1)
# magic constant: joint speed :)
# self.interface.set_joint_position_speed(0.9)
# if moveToInitial: self.initCam()
# initialize the desired position with the current position
# angles = self.interface.joint_angles()
# self.desired_joint_pos = { key: angles[key] for key in self.joint_names }
rospy.loginfo('Initialization finished!')
def test_multisensor_pose_jacobian(self):
configA = yaml.load('''
chain_id: chainA
before_chain: [transformA]
dh_link_num: 1
after_chain: [transformB]
''')
configB = yaml.load('''
chain_id: chainB
before_chain: [transformA]
dh_link_num: 1
after_chain: [transformB]
''')
robot_params = RobotParams()
robot_params.configure(
yaml.load('''
dh_chains:
chainA:
- [ 0, 0, 1, 0 ]
chainB:
- [ 0, 0, 2, 0 ]
tilting_lasers: {}
rectified_cams: {}
transforms:
transformA: [0, 0, 0, 0, 0, 0]
transformB: [0, 0, 0, 0, 0, 0]
checkerboards:
boardA:
corners_x: 2
corners_y: 2
spacing_x: 1
spacing_y: 1
'''))
free_dict = yaml.load('''
dh_chains:
chainA:
- [ 1, 0, 0, 0 ]
chainB:
- [ 0, 0, 0, 0 ]
tilting_lasers: {}
rectified_cams: {}
transforms:
transformA: [0, 0, 0, 0, 0, 0]
transformB: [0, 0, 0, 0, 0, 0]
checkerboards:
boardA:
spacing_x: 0
spacing_y: 0
''')
sensorA = ChainSensor(
configA,
ChainMeasurement(chain_id="chainA",
chain_state=JointState(position=[0])), "boardA")
sensorB = ChainSensor(
configB,
ChainMeasurement(chain_id="chainB",
chain_state=JointState(position=[0])), "boardA")
multisensor = MultiSensor(None)
multisensor.sensors = [sensorA, sensorB]
multisensor.checkerboard = "boardA"
pose_param_vec = numpy.array([0, 0, 0, 0, 0, 0])
calc = ErrorCalc(robot_params, free_dict, [])
expanded_param_vec = robot_params.deflate()
free_list = robot_params.calc_free(free_dict)
opt_param_mat = expanded_param_vec[numpy.where(free_list), 0].copy()
opt_param = numpy.array(opt_param_mat)[0]
J = calc.multisensor_pose_jacobian(opt_param, pose_param_vec,
multisensor)
print J
def reset_goal(self):
state = JointState()
state.name = op3_module_names
state.position = np.zeros(len(op3_module_names))
self.link_states_publisher.publish(state)
def publish_action2(self, action):
state = JointState()
state.name = op3_module_names
state.position = action[:len(op3_module_names)]
self.link_states_publisher.publish(state)
def poppy_node():
#start node
rospy.init_node('poppy_node')
rate = rospy.Rate(10)
#from creature param, create creature object
creature = rospy.get_param(rospy.get_name() + '/creature',
'poppy-humanoid')
global poppy
if creature.endswith(".json"):
import pypot.robot
poppy = pypot.robot.from_config(creature)
else:
libraryName = creature.replace('-', '_')
objectName = creature.replace('-', " ")
objectName = objectName.title()
objectName = objectName.replace(" ", "")
from importlib import import_module
mod = import_module(libraryName)
met = getattr(mod, objectName)
poppy = met()
#set motors info as params
for m in poppy.motors:
rospy.set_param(rospy.get_name() + '/motor/' + m.name + '/id', m.id)
rospy.set_param(rospy.get_name() + '/motor/' + m.name + '/model',
m.model)
rospy.set_param(rospy.get_name() + '/motor/' + m.name + '/direct',
m.direct)
rospy.set_param(rospy.get_name() + '/motor/' + m.name + '/offset',
m.offset)
rospy.set_param(rospy.get_name() + '/motor/' + m.name + '/upper_limit',
m.upper_limit)
rospy.set_param(rospy.get_name() + '/motor/' + m.name + '/lower_limit',
m.lower_limit)
#create publishers for motors present_position, present_speed, present_load and goal_position and goal_speed and max_torque
pubs = {}
pubs[rospy.get_name() + '/motors/read_present'] = rospy.Publisher(
rospy.get_name() + '/motors/read_present', JointState, queue_size=10)
pubs[rospy.get_name() + '/motors/read_goal'] = rospy.Publisher(
rospy.get_name() + '/motors/read_goal', JointState, queue_size=10)
#subscribe to topic to change the goal_position, goal_speed and compliance
rospy.Subscriber(rospy.get_name() + '/motors/write', JointState,
JointStateWrite)
for p in poppy.primitives:
rospy.Subscriber(rospy.get_name() + '/primitive/' + p.name + '/start',
String,
usePrimitive,
callback_args=[p.name, "start"])
rospy.Subscriber(rospy.get_name() + '/primitive/' + p.name + '/stop',
String,
usePrimitive,
callback_args=[p.name, "stop"])
while not rospy.is_shutdown():
msg = JointState()
msg.name = []
msg.position = []
msg.velocity = []
msg.effort = []
for m in poppy.motors:
msg.name.append(m.name)
msg.position.append(m.present_position)
msg.velocity.append(m.present_speed)
msg.effort.append(m.present_load)
pubs[rospy.get_name() + '/motors/read_present'].publish(msg)
msg = JointState()
msg.name = []
msg.position = []
msg.velocity = []
msg.effort = []
for m in poppy.motors:
msg.name.append(m.name)
msg.position.append(m.goal_position)
msg.velocity.append(m.goal_speed)
msg.effort.append(m.max_torque)
pubs[rospy.get_name() + '/motors/read_goal'].publish(msg)
rate.sleep()
def get_joint_state(cont):
joints = JointState()
joints.header.stamp = rospy.get_rostime()
joints.name = ["left_wheel", "right_wheel"]
joints.velocity = cont
return joints
#!/usr/bin/env python
import rospy
#from std_msgs.msg import String
from sensor_msgs.msg import JointState
#from std_msgs.msg import Float64MultiArray
from cisst_msgs.msg import vctDoubleVec
pub = rospy.Publisher('my_tor_node2', JointState) #, queue_size=10
msg = JointState()
def callback(data):
tup = data.data
rospy.loginfo(rospy.get_caller_id())
print(tup)
msg.header.seq = 0
msg.header.stamp.secs = 0
msg.header.stamp.nsecs = 0
msg.header.frame_id = ''
msg.name = []
msg.position = []
msg.velocity = []
msg.effort = tup
print(msg.effort)
pub.publish(msg)
def tor_listener():
# In ROS, nodes are uniquely named. If two nodes with the same
data=''
begin=time.time()
while 1:
#if you got some data, then timeout break
if total_data and time.time()-begin>timeout:
break
#if you got no data at all, wait a little longer
elif time.time()-begin>timeout*2:
break
try:
data=self.labview_sock.recv(8192)
if data:
total_data.append(data)
begin=time.time()
except:
pass
return ''.join(total_data)
if __name__ == '__main__':
rospy.init_node('labview_server')
server = LabviewServer()
while not rospy.is_shutdown():
msg_ros = JointState()
data = server.recv_timeout()
if data:
#~ fmt = '<IiiI5sIIssIssIssIssIssIssIssssI7d'
#~ msg_py = struct.unpack(fmt, data[:struct.calcsize(fmt)])
#~ print msg_py
print msg_ros.deserialize(data)
def test_full_jacobian(self):
configA = yaml.load('''
chain_id: chainA
before_chain: [transformA]
dh_link_num: 1
after_chain: []
''')
configB = yaml.load('''
chain_id: chainB
before_chain: []
dh_link_num: 1
after_chain: [transformB]
''')
robot_params = RobotParams()
robot_params.configure(
yaml.load('''
dh_chains:
chainA:
- [ 0, 0, 1, 0 ]
chainB:
- [ 0, 0, 2, 0 ]
tilting_lasers:
laserB:
before_joint: [0, 0, 0, 0, 0, 0]
after_joint: [0, 0, 0, 0, 0, 0]
rectified_cams: {}
transforms:
transformA: [0, 0, 0, 0, 0, 0]
transformB: [0, 0, 0, 0, 0, 0]
checkerboards:
boardA:
corners_x: 2
corners_y: 2
spacing_x: 1
spacing_y: 1
boardB:
corners_x: 1
corners_y: 1
spacing_x: 1
spacing_y: 1
'''))
free_dict = yaml.load('''
dh_chains:
chainA:
- [ 1, 0, 0, 0 ]
chainB:
- [ 0, 0, 0, 0 ]
tilting_lasers:
laserB:
before_joint: [1, 0, 0, 0, 0, 0]
after_joint: [0, 0, 0, 0, 0, 0]
rectified_cams: {}
transforms:
transformA: [0, 0, 0, 0, 0, 0]
transformB: [1, 0, 0, 0, 0, 0]
checkerboards:
boardA:
spacing_x: 1
spacing_y: 1
boardB:
spacing_x: 0
spacing_y: 0
''')
sensorA = ChainSensor(
configA,
ChainMeasurement(chain_id="chainA",
chain_state=JointState(position=[0])), "boardA")
sensorB = ChainSensor(
configB,
ChainMeasurement(chain_id="chainB",
chain_state=JointState(position=[0])), "boardB")
laserB = TiltingLaserSensor(
{'laser_id': 'laserB'},
LaserMeasurement(laser_id="laserB",
joint_points=[JointState(position=[0, 0, 2])]))
multisensorA = MultiSensor(None)
multisensorA.sensors = [sensorA]
multisensorA.checkerboard = "boardA"
multisensorA.update_config(robot_params)
multisensorB = MultiSensor(None)
multisensorB.sensors = [sensorB, laserB]
multisensorB.checkerboard = "boardB"
multisensorB.update_config(robot_params)
poseA = numpy.array([1, 0, 0, 0, 0, 0])
poseB = numpy.array([2, 0, 0, 0, 0, 0])
calc = ErrorCalc(robot_params, free_dict, [multisensorA, multisensorB])
expanded_param_vec = robot_params.deflate()
free_list = robot_params.calc_free(free_dict)
opt_param_mat = expanded_param_vec[numpy.where(free_list), 0].copy()
opt_param = numpy.array(opt_param_mat)[0]
opt_all_vec = concatenate([opt_param, poseA, poseB])
print "Calculating Sparse"
J = calc.calculate_jacobian(opt_all_vec)
numpy.savetxt("J_sparse.txt", J, fmt="% 4.3f")
#import code; code.interact(local=locals())
print "Calculating Dense"
from scipy.optimize.slsqp import approx_jacobian
J_naive = approx_jacobian(opt_all_vec, calc.calculate_error, 1e-6)
numpy.savetxt("J_naive.txt", J_naive, fmt="% 4.3f")
self.assertAlmostEqual(numpy.linalg.norm(J - J_naive), 0.0, 6)
def main():
print "INITIALIZING TORSO NODE IN SIMULATION MODE BY MARCOSOFT..."
###Connection with ROS
global pubGoalReached
rospy.init_node("torso")
br = tf.TransformBroadcaster()
jointStates = JointState()
jointStates.name = [
"spine_connect", "waist_connect", "shoulders_connect",
"shoulders_left_connect", "shoulders_right_connect"
]
jointStates.position = [0.0, 0.0, 0.0, 0.0, 0.0]
rospy.Subscriber("/hardware/torso/goal_pose", Float32MultiArray,
callbackGoalPose)
rospy.Subscriber("/hardware/torso/goal_rel_pose", Float32MultiArray,
callbackRelPose)
pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1)
pubCurrentPose = rospy.Publisher("/hardware/torso/current_pose",
Float32MultiArray,
queue_size=1)
pubGoalReached = rospy.Publisher("/hardware/torso/goal_reached",
Bool,
queue_size=1)
loop = rospy.Rate(60)
global goalSpine
global goalWaist
global goalShoulders
global spine
global waist
global shoulders
global newGoal
goalSpine = 0.0
goalWaist = 0.0
goalShoulders = 0.0
spine = 0
waist = 0
shoulders = 0
speedSpine = 0.005
speedWaist = 0.1
speedShoulders = 0.1
msgCurrentPose = Float32MultiArray()
msgGoalReached = Bool()
msgCurrentPose.data = [0, 0, 0]
newGoal = False
while not rospy.is_shutdown():
deltaSpine = goalSpine - spine
deltaWaist = goalWaist - waist
deltaShoulders = goalShoulders - shoulders
if deltaSpine > speedSpine:
deltaSpine = speedSpine
if deltaSpine < -speedSpine:
deltaSpine = -speedSpine
if deltaWaist > speedWaist:
deltaWaist = speedWaist
if deltaWaist < -speedWaist:
deltaWaist = -speedWaist
if deltaShoulders > speedShoulders:
deltaShoulders = speedShoulders
if deltaShoulders < -speedShoulders:
deltaShoulders = -speedShoulders
spine += deltaSpine
waist += deltaWaist
shoulders += deltaShoulders
jointStates.header.stamp = rospy.Time.now()
jointStates.position = [
spine, waist, shoulders, -shoulders, -shoulders
]
pubJointStates.publish(jointStates)
msgCurrentPose.data[0] = spine
msgCurrentPose.data[1] = waist
msgCurrentPose.data[2] = shoulders
pubCurrentPose.publish(msgCurrentPose)
if newGoal and abs(goalSpine - spine) < 0.02 and abs(
goalWaist - waist) < 0.05 and abs(goalShoulders -
shoulders) < 0.05:
newGoal = False
msgGoalReached.data = True
pubGoalReached.publish(msgGoalReached)
loop.sleep()
def test_single_sensor(self):
config = yaml.load('''
chain_id: chainA
before_chain: [transformA]
dh_link_num: 1
after_chain: [transformB]
''')
robot_params = RobotParams()
robot_params.configure(
yaml.load('''
dh_chains:
chainA:
- [ 0, 0, 1, 0 ]
chainB:
- [ 0, 0, 2, 0 ]
tilting_lasers: {}
rectified_cams: {}
transforms:
transformA: [0, 0, 0, 0, 0, 0]
transformB: [0, 0, 0, 0, 0, 0]
checkerboards:
boardA:
corners_x: 2
corners_y: 2
spacing_x: 1
spacing_y: 1
'''))
free_dict = yaml.load('''
dh_chains:
chainA:
- [ 1, 0, 1, 0 ]
chainB:
- [ 0, 0, 0, 0 ]
tilting_lasers: {}
rectified_cams: {}
transforms:
transformA: [1, 0, 0, 0, 0, 0]
transformB: [0, 0, 0, 0, 0, 0]
checkerboards:
boardA:
spacing_x: 1
spacing_y: 1
''')
chainM = ChainMeasurement(chain_id="chainA",
chain_state=JointState(position=[0]))
sensor = ChainSensor(config, chainM, "boardA")
sensor.update_config(robot_params)
target_pts = matrix([[1, 2, 1, 2], [0, 0, 1, 1], [0, 0, 0, 0],
[1, 1, 1, 1]])
calc = ErrorCalc(robot_params, free_dict, [])
expanded_param_vec = robot_params.deflate()
free_list = robot_params.calc_free(free_dict)
opt_param_mat = expanded_param_vec[numpy.where(free_list), 0].copy()
opt_param = numpy.array(opt_param_mat)[0]
J = calc.single_sensor_params_jacobian(opt_param, target_pts, sensor)
print J
def callback(transform):
rospy.loginfo(rospy.get_name() + "\nI'm doing ik\n")
# receive desired end effector positions in mm + theta
X = transform.translation.x
Y = transform.translation.y
Z = transform.translation.z
Theta = transform.rotation.w
print transform.translation
print transform.rotation
# kinematic constants in mm
E1, E2, RB, B2, A, B, D = 29.239, 29.239, 55.48, 34.1675, 111.76, 53.98, 8.255
J = [] # motor positions in radians
counter = 0
for m in range(0, 4):
if m == 0:
xg = -Y - E1 * (math.sin(Theta) - math.cos(Theta)) + E2
yg = X + E1 * (math.cos(Theta) + math.sin(Theta)) - B2
zg = Z
elif m == 1:
xg = X + E1 * (math.cos(Theta) + math.sin(Theta)) + E2
yg = Y + E1 * (math.sin(Theta) - math.cos(Theta)) + B2
zg = Z
elif m == 2:
xg = Y - E1 * (math.sin(Theta) - math.cos(Theta)) + E2
yg = -X + E1 * (math.cos(Theta) + math.sin(Theta)) - B2
zg = Z
elif m == 3:
xg = -X + E1 * (math.cos(Theta) + math.sin(Theta)) + E2
yg = -Y + E1 * (math.sin(Theta) - math.cos(Theta)) + B2
zg = Z
print "xg " + repr(xg) + " yg " + repr(yg) + " zg " + repr(zg)
temp = math.pow((A - 2 * D) * (A - 2 * D) - yg * yg, 0.5)
aPrimeSquared = (temp + 2 * D) * (temp + 2 * D)
c = math.pow((xg - RB) * (xg - RB) + zg * zg, 0.5)
#print (-aPrimeSquared+B*B+c*c)/(2*B*c)
try:
alpha = math.acos((-aPrimeSquared + B * B + c * c) / (2 * B * c))
print "worked " + repr(
(-aPrimeSquared + B * B + c * c) / (2 * B * c))
counter = counter + 1
except ValueError:
print "acos domain error " + repr(
(-aPrimeSquared + B * B + c * c) / (2 * B * c))
break
else:
beta = math.atan2(zg, xg - RB)
J.append(beta - alpha)
print "J:"
for j in J:
print " " + repr(j) # publish sensor_msgs/JointState to cmd_pos topic
if counter == 4:
# publish the 4 motor positions in radians
motors = ("1_A", "1_B", "2_A", "2_B")
velocity = [0, 0, 0, 0]
effort = [0, 0, 0, 0]
pub = rospy.Publisher('cmd_pos', JointState)
header = Header(0, rospy.get_rostime(), "")
jointState = JointState(header=header,
name=motors,
position=J,
velocity=velocity,
effort=effort)
pub.publish(jointState)
def publish_joint_states(self, position):
joint_states = JointState()
joint_states.header.stamp = rospy.Time.now()
joint_states.name = self._joint_names
joint_states.position = position
self._joint_states_pub.publish(joint_states)
def __init__(self, bot_name="NoName"):
# bot name
self.name = bot_name
print("self.name", self.name)
self.ImgDebug = False
# velocity publisher
self.vel_pub = rospy.Publisher('cmd_vel', Twist,queue_size=1)
# navigation publisher
self.client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
# Lidar
self.scan = LaserScan()
topicname_scan = "scan"
self.lidar_sub = rospy.Subscriber(topicname_scan, LaserScan, self.lidarCallback)
self.front_distance = DISTANCE_TO_ENEMY_INIT_VAL # init
self.front_scan = DISTANCE_TO_ENEMY_INIT_VAL
self.back_distance = DISTANCE_TO_ENEMY_INIT_VAL # init
self.back_scan = DISTANCE_TO_ENEMY_INIT_VAL
# usb camera
self.img = None
self.camera_preview = True
self.bridge = CvBridge()
topicname_image_raw = "image_raw"
self.image_sub = rospy.Subscriber(topicname_image_raw, Image, self.imageCallback)
self.red_angle = COLOR_TARGET_ANGLE_INIT_VAL # init
self.blue_angle = COLOR_TARGET_ANGLE_INIT_VAL # init
self.green_angle = COLOR_TARGET_ANGLE_INIT_VAL # init
self.red_distance = DISTANCE_TO_ENEMY_INIT_VAL # init
# FIND_ENEMY status
self.find_enemy = FIND_ENEMY_SEARCH
self.find_wait = 0
self.enemy_direct = 1
# joint
self.joint = JointState()
self.joint_sub = rospy.Subscriber('joint_states', JointState, self.jointstateCallback)
self.wheel_rot_r = 0
self.wheel_rot_l = 0
self.moving = False
# twist
self.x = 0;
self.th = 0;
# war status
topicname_war_state = "war_state"
self.war_state = rospy.Subscriber(topicname_war_state, String, self.stateCallback)
self.my_score = 0
self.enemy_score = 0
self.act_mode = ActMode.BASIC
# odom
topicname_odom = "odom"
self.odom = rospy.Subscriber(topicname_odom, Odometry, self.odomCallback)
# amcl pose
topicname_amcl_pose = "amcl_pose"
self.amcl_pose = rospy.Subscriber(topicname_amcl_pose, PoseWithCovarianceStamped, self.AmclPoseCallback)
# time
self.time_start = time.time()
def QuadMain():
quadcm_m = 1.0
quadmotor_m = 1.0
ball_m = 2.5
dt = 0.01 # timestep set to 10ms
# Create a new trep system - 6 generalized coordinates for quadrotor: x,y,z,roll,pitch,yaw
system = trep.System()
trep.potentials.Gravity(system, name="Gravity")
quadz = trep.Frame(system.world_frame, trep.TZ, "quadz")
quady = trep.Frame(quadz, trep.TY, "quady")
quadx = trep.Frame(quady, trep.TX, "quadx")
quadrx = trep.Frame(quadx, trep.RX, "quadrx", kinematic=True)
quadry = trep.Frame(quadrx, trep.RY, "quadry", kinematic=True)
# quadrz = trep.Frame(quadry,trep.RZ,"quadrz")
quadx.set_mass(quadcm_m) # central point mass
# # define motor positions and mass
# quad1 = trep.Frame(quadrz,trep.CONST_SE3,((1,0,0),(0,1,0),(0,0,1),(3,0,0)),"quad1")
# quad1.set_mass(quadmotor_m)
# quad2 = trep.Frame(quadrz,trep.CONST_SE3,((1,0,0),(0,1,0),(0,0,1),(-3,0,0)),"quad2")
# quad2.set_mass(quadmotor_m)
# quad3 = trep.Frame(quadrz,trep.CONST_SE3,((1,0,0),(0,1,0),(0,0,1),(0,3,0)),"quad3")
# quad3.set_mass(quadmotor_m)
# quad4 = trep.Frame(quadrz,trep.CONST_SE3,((1,0,0),(0,1,0),(0,0,1),(0,-3,0)),"quad4")
# quad4.set_mass(quadmotor_m)
# define ball frame
ballrx = trep.Frame(quadx, trep.RX, "ballrx", kinematic=False)
ballry = trep.Frame(ballrx, trep.RY, "ballry", kinematic=False)
ball = trep.Frame(ballry, trep.CONST_SE3,
((1, 0, 0), (0, 1, 0), (0, 0, 1), (0, 0, -1)), "ballcm")
ball.set_mass(ball_m)
# set thrust vector with input u1
trep.forces.BodyWrench(system,
quadry, (0, 0, 'u1', 0, 0, 0),
name='wrench1')
# # set four thrust vectors with inputs u1,u2,u3,u4
# trep.forces.BodyWrench(system,quad1,(0,0,'u1',0,0,0),name='wrench1')
# trep.forces.BodyWrench(system,quad2,(0,0,'u2',0,0,0),name='wrench2')
# trep.forces.BodyWrench(system,quad3,(0,0,'u3',0,0,0),name='wrench3')
# trep.forces.BodyWrench(system,quad4,(0,0,'u4',0,0,0),name='wrench4')
# set quadrotor initial altitude at 3m
system.get_config("quadz").q = 3.0
# Now we'll extract the current configuration into a tuple to use as
# initial conditions for a variational integrator.
q0 = system.q
# Create and initialize the variational integrator
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(0.0, q0, dt, q0)
# These are our simulation variables.
q = mvi.q2
t = mvi.t2
# u0=tuple([(4*quadmotor_m+quadcm_m+ball_m)/4*9.8,(4*quadmotor_m+quadcm_m+ball_m)/4*9.8,(4*quadmotor_m+quadcm_m+ball_m)/4*9.8,(4*quadmotor_m+quadcm_m+ball_m)/4*9.8])
# u=[u0]
u0 = np.array([(quadcm_m + ball_m) * 9.8])
u = tuple(u0)
# start ROS node to broadcast transforms to rviz
rospy.init_node('quad_simulator')
broadcaster = tf.TransformBroadcaster()
pub = rospy.Publisher('config', Float32MultiArray)
statepub = rospy.Publisher('joint_states', JointState)
jointstates = JointState()
configs = Float32MultiArray()
# subscribe to joystick topic from joy_node
rospy.Subscriber("joy", Joy, joycall)
r = rospy.Rate(100) # simulation rate set to 100Hz
# PD control variables
P = 200
D = 20
# Simulator Loop
while not rospy.is_shutdown():
# reset simulator if trigger pressed
if buttons[0] == 1:
mvi.initialize_from_configs(0.0, q0, dt, q0)
# # calculate thrust inputs
# u1 = u0[0] + P*(q[4]+0.1*axis[0]) + D*system.configs[4].dq - P*(q[0]-3.0) - D*system.configs[0].dq
# u2 = u0[1] - P*(q[4]+0.1*axis[0]) - D*system.configs[4].dq - P*(q[0]-3.0) - D*system.configs[0].dq
# u3 = u0[2] - P*(q[3]+0.1*axis[1]) - D*system.configs[3].dq - P*(q[0]-3.0) - D*system.configs[0].dq
# u4 = u0[3] + P*(q[3]+0.1*axis[1]) + D*system.configs[3].dq - P*(q[0]-3.0) - D*system.configs[0].dq
# u = tuple([u1,u2,u3,u4])
k = np.array([0.1 * axis[1], 0.1 * axis[0]])
# advance simluation one timestep using trep VI
mvi.step(mvi.t2 + dt, u, k)
q = mvi.q2
t = mvi.t2
configs.data = tuple(q) + u
jointstates.header.stamp = rospy.Time.now()
jointstates.name = ["q1ballrx", "q1ballry"]
jointstates.position = [q[3], q[4]]
jointstates.velocity = [system.configs[5].dq, system.configs[6].dq]
# send transform data to rviz
broadcaster.sendTransform(
(q[2], q[1], q[0]),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(), "quad1", "world")
broadcaster.sendTransform(
(0, 0, 0), tf.transformations.quaternion_from_euler(q[5], q[6], 0),
rospy.Time.now(), "quadr", "quad1")
statepub.publish(jointstates)
pub.publish(configs)
r.sleep()
def set_arm_joint(pub, joint_target):
joint_state = JointState()
joint_state.position = tuple(joint_target)
rospy.loginfo('Going to arm joint position: {}'.format(joint_state.position))
pub.publish(joint_state)
def run(self):
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
page = raw_input('Enter page: ')
if page == "recode":
motionPath = []
plan = ["init", 'PourStart', "init"]
self.engine.setPlan(plan)
s = time.time()
while self.engine.isRunning:
e = time.time()
if (e - s) > self.recodeTime:
## recode Path
motionPath.append(self.joint_states)
s = time.time()
self.engine.run()
motionPath = np.array(motionPath)
np.save(
os.path.dirname(__file__) + "/PathData/path.npy",
motionPath)
rate.sleep()
if page == "learn":
jname = [
"l_arm_sh_p1", "l_arm_sh_r", "l_arm_sh_p2", "l_arm_el_y",
"l_arm_wr_r", "l_arm_wr_y", "l_arm_wr_p"
]
self.engine.set_direct_control(jname)
motionPath = np.load(
os.path.dirname(__file__) + "/PathData/path.npy")
dmp = dmp_discrete.DMPs_discrete(n_dmps=7,\
n_bfs=500, \
ay=np.ones(7)*10.0,\
dt = 1.0/len(motionPath),\
)
dmp.imitate_path(motionPath.T, plot=False)
dmp.reset_state()
dmp.y0 = motionPath[0]
dmp.goal = motionPath[-1]
y, dy, ddy = dmp.rollout()
joint = JointState()
joint.name = jname
for i in range(len(motionPath)):
joint.position = y[i]
self.engine.set_joint(joint)
time.sleep(self.recodeTime)
if page == "load":
jname = [
"l_arm_sh_p1", "l_arm_sh_r", "l_arm_sh_p2", "l_arm_el_y",
"l_arm_wr_r", "l_arm_wr_y", "l_arm_wr_p"
]
joint = JointState()
joint.name = jname
motionPath = np.load(
os.path.dirname(__file__) + "/PathData/path.npy")
for i in range(len(motionPath)):
joint.position = motionPath[i]
self.engine.set_joint(joint)
time.sleep(self.recodeTime)
print "No data for " + name
if __name__ == "__main__":
joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1)
rospy.init_node('real_joint_state_publisher', anonymous=True)
rate = rospy.Rate(100)
rospy.wait_for_service('/gazebo/get_joint_properties')
while not rospy.is_shutdown():
joint_state_msg = JointState()
joint_state_msg.header.stamp = rospy.Time.now()
try:
get_joint_data = rospy.ServiceProxy('/gazebo/get_joint_properties',
GetJointProperties)
populate_joint_state_msg(joint_state_msg, get_joint_data, "joint1")
populate_joint_state_msg(joint_state_msg, get_joint_data, "joint2")
print "Publishing data for both joints"
except rospy.ServiceException, e:
print "Service call failed: %s" % e
#zeroing out the estimates
def __init__(self,
prefix="",
gripper="",
interface='eth0',
jaco_ip="10.66.171.15",
dof=""):
"""
Setup a lock for accessing data in the control loop
"""
self._lock = threading.Lock()
"""
Assume success until posted otherwise
"""
rospy.loginfo('Starting JACO2 control')
self.init_success = True
self._prefix = prefix
self.iface = interface
self.arm_dof = dof
"""
List of joint names
"""
if ("6dof" == self.arm_dof):
self._joint_names = [
self._prefix + '_shoulder_pan_joint',
self._prefix + '_shoulder_lift_joint',
self._prefix + '_elbow_joint', self._prefix + '_wrist_1_joint',
self._prefix + '_wrist_2_joint',
self._prefix + '_wrist_3_joint'
]
elif ("7dof" == self.arm_dof):
self._joint_names = [
self._prefix + '_shoulder_pan_joint',
self._prefix + '_shoulder_lift_joint',
self._prefix + '_arm_half_joint',
self._prefix + '_elbow_joint',
self._prefix + '_wrist_spherical_1_joint',
self._prefix + '_wrist_spherical_2_joint',
self._prefix + '_wrist_3_joint'
]
else:
rospy.logerr(
"DoF needs to be set 6 or 7, cannot start SIArmController")
return
self._num_joints = len(self._joint_names)
"""
Create the hooks for the API
"""
if ('left' == prefix):
self.api = KinovaAPI('left', self.iface, jaco_ip, '255.255.255.0',
24000, 24024, 44000, self.arm_dof)
elif ('right' == prefix):
self.api = KinovaAPI('right', self.iface, jaco_ip, '255.255.255.0',
25000, 25025, 55000, self.arm_dof)
else:
rospy.logerr(
"prefix needs to be set to left or right, cannot start the controller"
)
return
if not (self.api.init_success):
self.Stop()
return
self.api.SetCartesianControl()
self._position_hold = False
self.estop = False
"""
Initialize the joint feedback
"""
pos = self.api.get_angular_position()
vel = self.api.get_angular_velocity()
force = self.api.get_angular_force()
self._joint_fb = dict()
self._joint_fb['position'] = pos[:self._num_joints]
self._joint_fb['velocity'] = vel[:self._num_joints]
self._joint_fb['force'] = force[:self._num_joints]
if ("kg2" == gripper):
self._gripper_joint_names = [
self._prefix + '_gripper_finger1_joint',
self._prefix + '_gripper_finger2_joint'
]
self.num_fingers = 2
elif ("kg3" == gripper):
self._gripper_joint_names = [
self._prefix + '_gripper_finger1_joint',
self._prefix + '_gripper_finger2_joint',
self._prefix + '_gripper_finger3_joint'
]
self.num_fingers = 3
if (0 != self.num_fingers):
self._gripper_fb = dict()
self._gripper_fb['position'] = pos[self.
_num_joints:self._num_joints +
self.num_fingers]
self._gripper_fb['velocity'] = vel[self.
_num_joints:self._num_joints +
self.num_fingers]
self._gripper_fb['force'] = force[self.
_num_joints:self._num_joints +
self.num_fingers]
"""
Register the publishers and subscribers
"""
self.last_teleop_cmd_update = rospy.get_time() - 0.5
self._teleop_cmd_sub = rospy.Subscriber(
"/movo/%s_arm/cartesian_vel_cmd" % self._prefix,
JacoCartesianVelocityCmd, self._update_teleop_cmd)
self._gripper_cmd = 0.0
self._ctl_mode = AUTONOMOUS_CONTROL
self._jstpub = rospy.Publisher("/movo/%s_arm_controller/state" %
self._prefix,
JointTrajectoryControllerState,
queue_size=10)
self._jstmsg = JointTrajectoryControllerState()
self._jstmsg.header.seq = 0
self._jstmsg.header.frame_id = ''
self._jstmsg.header.stamp = rospy.get_rostime()
self._jspub = rospy.Publisher("/movo/%s_arm/joint_states" %
self._prefix,
JointState,
queue_size=10)
self._jsmsg = JointState()
self._jsmsg.header.seq = 0
self._jsmsg.header.frame_id = ''
self._jsmsg.header.stamp = rospy.get_rostime()
self._jsmsg.name = self._joint_names
self._actfdbk_pub = rospy.Publisher("/movo/%s_arm/actuator_feedback" %
self._prefix,
KinovaActuatorFdbk,
queue_size=10)
self._actfdbk_msg = KinovaActuatorFdbk()
self._jsmsg.header.seq = 0
self._jsmsg.header.frame_id = ''
self._jsmsg.header.stamp = rospy.get_rostime()
if (0 != self.num_fingers):
self._teleop_gripper_cmd_sub = rospy.Subscriber(
"/movo/%s_gripper/vel_cmd" % self._prefix, Float32,
self._update_teleop_gripper_cmd)
self._gripper_jspub = rospy.Publisher(
"/movo/%s_gripper/joint_states" % self._prefix,
JointState,
queue_size=10)
self._gripper_jsmsg = JointState()
self._gripper_jsmsg.header.seq = 0
self._gripper_jsmsg.header.frame_id = ''
self._gripper_jsmsg.header.stamp = rospy.get_rostime()
self._gripper_jsmsg.name = self._gripper_joint_names
"""
This starts the controller in cart vel mode so that teleop is active by default
"""
if (0 != self.num_fingers):
self._gripper_pid = [None] * self.num_fingers
for i in range(self.num_fingers):
self._gripper_pid[i] = JacoPID(5.0, 0.0, 0.8)
self._gripper_vff = DifferentiateSignals(
self.num_fingers, self._gripper_fb['position'])
self._gripper_rate_limit = RateLimitSignals(
[FINGER_ANGULAR_VEL_LIMIT] * self.num_fingers,
self.num_fingers, self._gripper_fb['position'])
if ("6dof" == self.arm_dof):
self._arm_rate_limit = RateLimitSignals(JOINT_6DOF_VEL_LIMITS,
self._num_joints,
self._joint_fb['position'])
if ("7dof" == self.arm_dof):
self._arm_rate_limit = RateLimitSignals(JOINT_7DOF_VEL_LIMITS,
self._num_joints,
self._joint_fb['position'])
self._arm_vff_diff = DifferentiateSignals(self._num_joints,
self._joint_fb['position'])
self._pid = [None] * self._num_joints
for i in range(self._num_joints):
self._pid[i] = JacoPID(5.0, 0.0, 0.8)
"""
self._pid[0] = JacoPID(5.0,0.0,0.8)
self._pid[1] = JacoPID(5.0,0.0,0.8)
self._pid[2] = JacoPID(5.0,0.0,0.8)
self._pid[3] = JacoPID(5.0,0.0,0.8)
self._pid[4] = JacoPID(5.0,0.0,0.8)
self._pid[5] = JacoPID(5.0,0.0,0.8)
"""
self.pause_controller = False
self._init_ext_joint_position_control()
self._init_ext_gripper_control()
"""
Set temporary tucked position after homing
"""
"""
if 'left' == self._prefix:
self._arm_cmds['position'][1]+= deg_to_rad(80.0)
self._arm_cmds['position'][2]+= deg_to_rad(50.0)
self._arm_cmds['position'][4]-= deg_to_rad(90.0)
else:
self._arm_cmds['position'][1]-= deg_to_rad(80.0)
self._arm_cmds['position'][2]-= deg_to_rad(50.0)
self._arm_cmds['position'][4]+= deg_to_rad(90.0)
"""
"""
Update the feedback once to get things initialized
"""
self._update_controller_data()
"""
Start the controller
"""
rospy.loginfo("Starting the %s controller" % self._prefix)
self._done = False
self._t1 = rospy.Timer(rospy.Duration(0.01), self._run_ctl)
def main():
rospy.init_node('jointInterface', anonymous=True)
rate = rospy.Rate(CONTROL_LOOP_FREQ)
global device
global command_queue
# Find and connect to the stepper motor controller
port = sys.argv[1]
s = serial.Serial(port=port, baudrate=COMM_DEFAULT_BAUD_RATE, timeout=0.05)
print s.BAUDRATES
device = BLDCControllerClient(s)
for key in mapping:
device.leaveBootloader(key)
pubArray = {}
pubCurrArray = {}
subArray = {}
for key in mapping:
pubArray[key] = rospy.Publisher("/DOF/" + mapping[key] + "_State",
JointState,
queue_size=10)
pubCurrArray[key] = rospy.Publisher("/DOF/" + mapping[key] +
"_Current",
Float32,
queue_size=10)
subArray[key] = rospy.Subscriber("/DOF/" + mapping[key] + "_Cmd",
Float64,
makeSetCommand(key),
queue_size=1)
# Set up a signal handler so Ctrl-C causes a clean exit
def sigintHandler(signal, frame):
# device.setParameter('iq_s', 0) ###############################################################--> must fix at some point
# device.setControlEnabled(False) ################################################################--> must fix at some point
print 'quitting'
sys.exit()
signal.signal(signal.SIGINT, sigintHandler)
# Set current to zero, enable current control loop
for key in mapping:
# device.setParameter(key, 'id_s', 0)
# device.setParameter(key, 'iq_s', 0)
device.setControlEnabled(
key, 0
) ########################### change to 1 when you want to actually control
#angle = 0.0 # Treat the starting position as zero
#last_mod_angle = getEncoderAngleRadians(device)
start_time = time.time()
time_previous = start_time
angle_previous_mod = {}
angle_accumulated = {}
time.sleep(1)
for key in mapping:
angle_previous_mod[key] = getEncoderAngleRadians(device, key)
angle_accumulated[key] = 0.0
r = rospy.Rate(30)
while not rospy.is_shutdown():
for key in mapping:
try:
# Compute the desired setpoint for the current time
jointName = mapping[key]
loop_start_time = time.time()
mod_angle = getEncoderAngleRadians(device, key)
delta_time = loop_start_time - time_previous
time_previous = loop_start_time
delta_angle = (mod_angle - angle_previous_mod[key] +
math.pi) % (2 * math.pi) - math.pi
angle_previous_mod[key] = mod_angle
angle_accumulated[key] = angle_accumulated[key] + delta_angle
jointMsg = JointState()
jointMsg.name = [jointName]
jointMsg.position = [mod_angle]
#jointMsg.velocity = [device.getVelocity(key)]
jointMsg.effort = [0.0]
pubArray[key].publish(jointMsg)
print("name: " + jointName + " position: " +
str(jointMsg.position))
time.sleep(
max(
0.0, loop_start_time + 1.0 / CONTROL_LOOP_FREQ -
time.time()))
currMsg = Float32()
currMsg.data = float(0)
pubCurrArray[key].publish(currMsg)
except Exception as e:
print(e)
pass
if key in command_queue:
device.setDuty(key, command_queue[key])
command_queue = {}
r.sleep()
#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import JointState
import math
from inverse_problem_srv.srv import publish_cmd, publish_cmdResponse
import numpy as np
jointPub = rospy.Publisher('/angle_robot/joint_states_kinematic',
JointState,
queue_size=100)
currentState = JointState()
last_gripper = 0
currentState.position = [0, 0, 0, 0, 0, 0]
maxVelocity = 1.5
gripper_vel = 2
gripper_pos = 20
countOfJoint = 6
iter_time = 0.01
gripperEf = 0
def realize_of_principle(goalPoseMsg, jointName, timeOfWay):
global currentState
q = []
time = []
time.append(0)
i = 0
k = 0
start = np.array(currentState.position[0:countOfJoint - 1])
end = np.array(goalPoseMsg)
def handle(self):
self.socket_lock = threading.Lock()
self.last_joint_states = None
setConnectedRobot(self)
print "Handling a request"
try:
buf = self.recv_more()
if not buf: return
while True:
#print "Buf:", [ord(b) for b in buf]
# Unpacks the message type
mtype = struct.unpack_from("!i", buf, 0)[0]
buf = buf[4:]
#print "Message type:", mtype
if mtype == MSG_OUT:
# Unpacks string message, terminated by tilde
i = buf.find("~")
while i < 0:
buf = buf + self.recv_more()
i = buf.find("~")
if len(buf) > 2000:
raise Exception(
"Probably forgot to terminate a string: %s..."
% buf[:150])
s, buf = buf[:i], buf[i + 1:]
log("Out: %s" % s)
elif mtype == MSG_JOINT_STATES:
while len(buf) < 3 * (6 * 4):
buf = buf + self.recv_more()
state_mult = struct.unpack_from("!%ii" % (3 * 6), buf, 0)
buf = buf[3 * 6 * 4:]
state = [s / MULT_jointstate for s in state_mult]
msg = JointState()
msg.header.stamp = rospy.get_rostime()
msg.name = joint_names
msg.position = [0.0] * 6
for i, q_meas in enumerate(state[:6]):
msg.position[i] = q_meas + joint_offsets.get(
joint_names[i], 0.0)
msg.velocity = state[6:12]
msg.effort = state[12:18]
self.last_joint_states = msg
pub_joint_states.publish(msg)
elif mtype == MSG_WRENCH:
while len(buf) < (6 * 4):
buf = buf + self.recv_more()
state_mult = struct.unpack_from("!%ii" % (6), buf, 0)
buf = buf[6 * 4:]
state = [s / MULT_wrench for s in state_mult]
wrench_msg = WrenchStamped()
wrench_msg.header.stamp = rospy.get_rostime()
wrench_msg.wrench.force.x = state[0]
wrench_msg.wrench.force.y = state[1]
wrench_msg.wrench.force.z = state[2]
wrench_msg.wrench.torque.x = state[3]
wrench_msg.wrench.torque.y = state[4]
wrench_msg.wrench.torque.z = state[5]
pub_wrench.publish(wrench_msg)
elif mtype == MSG_QUIT:
print "Quitting"
raise EOF("Received quit")
elif mtype == MSG_WAYPOINT_FINISHED:
while len(buf) < 4:
buf = buf + self.recv_more()
waypoint_id = struct.unpack_from("!i", buf, 0)[0]
buf = buf[4:]
print "Waypoint finished (not handled)"
else:
raise Exception("Unknown message type: %i" % mtype)
if not buf:
buf = buf + self.recv_more()
except EOF, ex:
print "Connection closed (command):", ex
setConnectedRobot(None)
def _pub_joint_velocities(self, velocities):
joint_state = JointState()
joint_state.velocity = tuple(velocities)
self.joint_pub.publish(joint_state)
