I want to add an INS to tf which will transform pointcloud from ORB SLAM2 to real world coordinate. My idea is to do a transformation so that the INS is at the rotational center of the robot.
My tf_tree looks like this: INS -> map -> base_link -> camera_link
My problem - despite the set transformation, the point cloud is saved in the orb slam system, not in the real coordinate system with INS. For my future work I have to get point cloud with world coordinates in ECEF.
(Ubuntu 18.04 with ROS Melodic and orb_slam2__ros)
What I made:
Between INS and map I made a broadcaster that reads the data from IMU (x,y,z,w) and transforms it to ROS msg.
transform_msg.header.frame_id = "INS"
transform_msg.child_frame_id = "map"
If I understand correctly, ORB SLAM2 transforms the point cloud from base_link to the map. (These parameters can be changed na file Node.cc na file .launch of camera in OS2)
Between base_link and camera_link I have a static_transform_publisher.
node pkg="tf2_ros" type="static_transform_publisher" name="gopro_offset" args="0 0 0 0 0 0 base_link camera_link" / include file="$(find orb_slam2_ros)/ros/launch/GOPRO.launch" /
Related
I'm a student teaching myself Drake, specifically pydrake with Dr. Russ Tedrake's excellent Underactuated Robotics course. I am trying to write a combined energy shaping and lqr controller for keeping a cartpole system balanced upright. I based the diagram on the cartpole example found in Chapter 3 of Underactuated Robotics [http://underactuated.mit.edu/acrobot.html], and the SwingUpAndBalanceController on Chapter 2: [http://underactuated.mit.edu/pend.html].
I have found that due to my use of the cart_pole.sdf model I have to create an abstract input port due receive FramePoseVector from the cart_pole.get_output_port(0). From there I know that I have to create a control signal output of type BasicVector to feed into a Saturation block before feeding into the cartpole's actuation port.
The problem I'm encountering right now is that I'm not sure how to get the system's current state data in the DeclareVectorOutputPort's callback function. I was under the assumption I would use the LeafContext parameter in the callback function, OutputControlSignal, obtaining the BasicVector continuous state vector. However, this resulting vector, x_bar is always NaN. Out of desperation (and testing to make sure the rest of my program worked) I set x_bar to the controller's initialization cart_pole_context and have found that the simulation runs with a control signal of 0.0 (as expected). I can also set output to 100 and the cartpole simulation just flies off into endless space (as expected).
TL;DR: What is the proper way to obtain the continuous state vector in a custom controller extending LeafSystem with a DeclareVectorOutputPort?
Thank you for any help! I really appreciate it :) I've been teaching myself so it's been a little arduous haha.
# Combined Energy Shaping (SwingUp) and LQR (Balance) Controller
# with a simple state machine
class SwingUpAndBalanceController(LeafSystem):
def __init__(self, cart_pole, cart_pole_context, input_i, ouput_i, Q, R, x_star):
LeafSystem.__init__(self)
self.DeclareAbstractInputPort("state_input", AbstractValue.Make(FramePoseVector()))
self.DeclareVectorOutputPort("control_signal", BasicVector(1),
self.OutputControlSignal)
(self.K, self.S) = BalancingLQRCtrlr(cart_pole, cart_pole_context,
input_i, ouput_i, Q, R, x_star).get_LQR_matrices()
(self.A, self.B, self.C, self.D) = BalancingLQRCtrlr(cart_pole, cart_pole_context,
input_i, ouput_i,
Q, R, x_star).get_lin_matrices()
self.energy_shaping = EnergyShapingCtrlr(cart_pole, x_star)
self.energy_shaping_context = self.energy_shaping.CreateDefaultContext()
self.cart_pole_context = cart_pole_context
def OutputControlSignal(self, context, output):
#xbar = copy(self.cart_pole_context.get_continuous_state_vector())
xbar = copy(context.get_continuous_state_vector())
xbar_ = np.array([xbar[0], xbar[1], xbar[2], xbar[3]])
xbar_[1] = wrap_to(xbar_[1], 0, 2.0*np.pi) - np.pi
# If x'Sx <= 2, then use LQR ctrlr. Cost-to-go J_star = x^T * S * x
threshold = np.array([2.0])
if (xbar_.dot(self.S.dot(xbar_)) < 2.0):
#output[:] = -self.K.dot(xbar_) # u = -Kx
output.set_value(-self.K.dot(xbar_))
else:
self.energy_shaping.get_input_port(0).FixValue(self.energy_shaping_context,
self.cart_pole_context.get_continuous_state_vector())
output_val = self.energy_shaping.get_output_port(0).Eval(self.energy_shaping_context)
output.set_value(output_val)
print(output)
Here are two things that might help:
If you want to get the state of the cart-pole from MultibodyPlant, you probably want to be connecting to the continuous_state output port, which gives you a normal vector instead of the abstract-type FramePoseVector. In that case, your call to get_input_port().Eval(context) should work just fine.
If you do really want to read the FramePoseVector, then you have to evaluate the input port slightly differently. You can find an example of that here.
I have a problem with my 5 inch resistive touch screen which I need to calibrate touch on Kivy app. My touch in Raspberry pi is correctly calibrated but when it comes to Kivy app it is not correct.
How can I calibrate the touch to get accurate touch in my Kivy app (GUI). I am using Raspberry pi 3 with Raspbian os (full version not lite) this.
Take a look at the configuration options for Kivy's mtdev input module: https://kivy.org/docs/api-kivy.input.providers.mtdev.html - in particular max_position_x, min_position_y, etc. From the documentation:
You can use a custom range for the X, Y and pressure values. On some drivers, the range reported is invalid. To fix that, you can add these options to the argument line:
invert_x : 1 to invert X axis
invert_y : 1 to invert Y axis
min_position_x : X minimum
max_position_x : X maximum
min_position_y : Y minimum
max_position_y : Y maximum
min_pressure : pressure minimum
max_pressure : pressure maximum
min_touch_major : width shape minimum
max_touch_major : width shape maximum
min_touch_minor : width shape minimum
max_touch_minor : height shape maximum
rotation : 0,90,180 or 270 to rotate
In ~/.kivy/config.ini, you should put something like this:
[input]
mtdev_%(name)s = probesysfs,provider=mtdev,max_position_x=12345,max_position_y=12345
This is if you're using mtdev, but you can also use hidinput from kernel 2.6.34 onwards. See also: https://kivy.readthedocs.io/en/master/api-kivy.input.providers.hidinput.html
I am trying to learn linearK estimates on a small linnet object from the CRC spatstat book (chapter 17) and when I use the linearK function, spatstat throws an error. I have documented the process in the comments in the r code below. The error is as below.
Error in seq.default(from = 0, to = right, length.out = npos + 1L) : 'to' cannot be NA, NaN or infinite
I do not understand how to resolve this. I am following this process:
# I have data of points for each data of the week
# d1 is district 1 of the city.
# I did the step below otherwise it was giving me tbl class
d1_data=lapply(split(d1, d1$openDatefactor),as.data.frame)
# I previously create a linnet and divided it into districts of the city
d1_linnet = districts_linnet[["d1"]]
# I create point pattern for each day
d1_ppp = lapply(d1_data, function(x) as.ppp(x, W=Window(d1_linnet)))
plot(d1_ppp[[1]], which.marks="type")
# I am then converting the point pattern to a point pattern on linear network
d1_lpp <- as.lpp(d1_ppp[[1]], L=d1_linnet, W=Window(d1_linnet))
d1_lpp
Point pattern on linear network
3 points
15 columns of marks: ‘status’, ‘number_of_’, ‘zip’, ‘ward’,
‘police_dis’, ‘community_’, ‘type’, ‘days’, ‘NAME’,
‘DISTRICT’, ‘openDatefactor’, ‘OpenDate’, ‘coseDatefactor’,
‘closeDate’ and ‘instance’
Linear network with 4286 vertices and 6183 lines
Enclosing window: polygonal boundary
enclosing rectangle: [441140.9, 448217.7] x [4640080, 4652557] units
# the errors start from plotting this lpp object
plot(d1_lpp)
"show.all" is not a graphical parameter
Show Traceback
Error in plot.window(...) : need finite 'xlim' values
coords(d1_lpp)
x y seg tp
441649.2 4649853 5426 0.5774863
445716.9 4648692 5250 0.5435492
444724.6 4646320 677 0.9189631
3 rows
And then consequently, I also get error on linearK(d1_lpp)
Error in seq.default(from = 0, to = right, length.out = npos + 1L) : 'to' cannot be NA, NaN or infinite
I feel lpp object has the problem, but I find it hard to interpret the errors and how to resolve them. Could someone please guide me?
Thanks
I can confirm there is a bug in plot.lpp when trying to plot the marked point pattern on the linear network. That will hopefully be fixed soon. You can plot the unmarked point pattern using
plot(unmark(d1_lpp))
I cannot reproduce the problem with linearK. Which version of spatstat are you running? In the development version on my laptop spatstat_1.51-0.073 everything works. There has been changes to this code recently, so it is likely that this will be solved by updating to development version (see https://github.com/spatstat/spatstat).
I am creating a Solar Array Simulator with python to simulate the voltage and current being created with different angles and intensities of sunlight as a satellite orbits around the earth. I have a very simple program that outputs the voltage and current just with the angle (no orbiting parameters yet). However, I need it to communicate the outputs generated with a E4350B model solar array simulator through a serial port, and I don't know where to start. I have installed pip and used that to install PySerial but do not know what to do from there. How do I communicate the voltage and amp outputs to the simulator through COM ports? Here is what I have for my program that runs the simulator.
from math import sin,radians,pi
import time
'''Needed information:
Voc (per one cell) = 2,680mV
Vmp (per one cell = 2,325mV
Isc (per one cell) = 453mA
Imp (per one cell) = 434mA
angular velocity = .05d/s'''
#Timer eclipse set for total eclipse tim ein seconds
timeEclipse = 1800
#total eclipse time = 30 mins
#Timer sun set for how many seconds it takes to change one degree
timeSun = 20
#Total sun exposer time = 60 mins
#Find the Vmp, Voc, Imp, and Isc from Beta angles 0 - 180
def Exposure():
tSun = timeSun
for x in range(0,181):
angle = sin(radians(x))
Voc = 2680 * angle
Vmp = 2325 * angle
#Amps are going to be a function of voltage
Isc = 453 * angle
Imp = 434 * angle
#
print('At angle ',x,' Vmp = ',Vmp,
'mV, Voc = ',Voc,'mV, Isc = ',Isc,'mA, and Imp = ',Imp,'mA')
time.sleep(tSun)
#Simulate time during eclipse
#Outputs nothing
def Eclipse():
tEclipse = timeEclipse
time.sleep(tEclipse)
#Run loop through Exposure and eclipse
def Run():
run = True
while(run):
Exposure()
Eclipse()
P.S. For anybody who dabbles in a little bit of physics, I need a way to find the current as a function of the voltage at every angle.
I tried to interface a RaspBerry pi with a LM335 temperature sensor this week-end. I'm using a MCP 3208 micro controller (channel 0) to interface the sensor. My goal is to collect samples data in SPI mode with python 3 scripts (classes).
I've checked the wiring and everything seems OK for me, I'but I'am a beginner, not really aware of Electronic concepts.
On the software side , I've installed quick2wire that claims to be python 3 compatible. In fact I want to lead the micro-controller with Python 3 API's (not thru shell calls)
Components
Raspberry pi REV2 model B with Rasbian-wheezy / Quick2wire installed. /dev/spix.y devices are listed.
MCP3208 ADC : 12 bits ADC / SPI. I'm using CS0 from the GPIO. The sensor is connected to channel 0 (B). see datasheet.
LM335 : temperature sensor. Outputs 10mV / °K. Min 5muA / Max 5 mA. It's connected to the MCP3208 channel #0 (A). see datasheet
220 ohms resistor (C). set up regarding LM335 outputs and desired temperature range coverage with my own calculations : May be a problem ...
Schematics extract
The LM335 (zener diode like) is connected as :
Wiring
Components are wired as shown bellow. Note that the yellow link is connected behind the cobbler kit on the CS0 SPI channel.
Quick2wire
I use the bellow script to query the CS0/Channel 0 GPIO interface. Unfortunately, I've not found usefull informations on the quick2wire-python-api API's. I've just copy/paste an example found as it was written in the same goal. I'm not sure if it really works :
#!/usr/bin/env python3
from quick2wire.spi import *
import sys, time
try:
channel = int(sys.argv[1])
except:
channel = 0
MCP3208 = SPIDevice(channel, 0)
while True:
try:
response = MCP3208.transaction(writing_bytes(0x41, 0x13), reading(1))
print ("output = %i" % ord(response[0]))
time.sleep(1)
except KeyboardInterrupt:
break
The script outputs :
output = 0
output = 0
output = 0
output = 0
output = 0
....
The result is the same with the channel 1 ( with argv = 1)
As the MCP3208 Din (probe output) receives voltage (see bellow) quick2wire should read at 18°C (rawghly my home inside temperature today)
3,3 V / 2^12 = 805 muA as I understand as "digital step"
18°C + 273°C = 291 => 2,91 V on the micro controller Din pin
and then return 2 910 / 0.805 = 3 615
Am I wrong ?
Controls
I've no oscilloscope, the only measures I can read are :
Voltage is 2.529 V at B checkpoint and 0,5 V (+/-5%) on the other MCP3208 channels
Note : the adjust pin is not used on the LM335 so results way not be accurate but voltage is here !
Seems to be a problem on the quick2wire side I think. But which ?
Code
The quick2wire.spi.SPIDevice class lakes of détails on the transfers parameter in terms of structure, content and output response format.
def transaction(self, *transfers):
"""
Perform an SPI I/O transaction.
Arguments:
*transfers -- SPI transfer requests created by one of the reading,
writing, writing_bytes, duplex or duplex_bytes
functions.
Returns: a list of byte sequences, one for each read or duplex
operation performed.
"""
transfer_count = len(transfers)
ioctl_arg = (spi_ioc_transfer*transfer_count)()
# populate array from transfers
for i, transfer in enumerate(transfers):
ioctl_arg[i] = transfers[i].to_spi_ioc_transfer()
ioctl(self.fd, SPI_IOC_MESSAGE(transfer_count), addressof(ioctl_arg))
return [transfer.to_read_bytes() for t in transfers if t.has_read_buf]
Another question :
how to set SPI configuration values like mode, clock speed, bits per word, LSB ... and so on.
Thanks in advance for your help.
I know you probably intend to learn how to use the ADC, an so this isn't really an answer to your question (I will use your very rich post for sure - thanks), but I'm aware of temperature sensors that already pack data in GPIO serial line, that are best suited for the raspberry.
You really have to read this awesome tutorial, if you haven't already.